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ABSTRACT 



In this work, the control laws of a bank- to^ turn mis- 
sile using and optimal estimator in the terminal guidance 
phase were designed, and the effect of increasing the number 
of measurement sensors in the missile to generate more 
information on the state was in\ r es t igated . In the design of 
the control law the modern optimal control theory with the a 
quadratic performance index was used. Implementation of 
this control law required the use of a Kalman filter as the 
optimal estimator. The extended Kalman filter algorithm was 
utilized in the present study since the measurement states 
were non-linear functions of the state vectors. In order to 
test the effects of the implementation of the increased mea- 
surement sensors, two-, four-, and six-measurement sensors 
were assumed to be implemented in the optimal estimator. By 
computer analysis, the designed guidance laws were evaluated 
and the effect of the implementation of increased measurement 
sensors was tested. 

The results of the simulation revealed that the designed 
guidance law was successful within the specified scenarios, 
the effect of the implementation of increased measurement 
sensors for the estimator was favorable only in that 
increased measurement sensors generated more information 
about the state vectors. 
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I. INTRODUCTION 



The bank-to-turn missile has high lift acceleration in 
a direction perpendicular to its wings. For airbreathing 
missiles which are required for large stand-off ranges it 
also offers the advantage of lower inlet angles of attack 
than that of skid-to-turn missiles. 

Although the control laws using the modern guidance 
control theory are more complex than the normally used 
proportional navigation air-to-ground method, they have 
great potential for maneuvering targets in air-to-air 
engagement situations. The application of optimal control 
and estimation theory to bank-to-turn missile configurations 
has been tried in order to obtain increased performance. In 
the application of the optimal control theory to the bank- 
to-turn missile, it is necessary to have information on all 
states or an estimate of the state variables that have not 
been measured. Since the state information available from 
the typical missile sensors is limited, it is necessary to 
employ an estimator. The estimator used in this study was 
a first order extended Kalman filter. 

The present work addressed the design and evaluation of 
the optimal state estimator and optimal control laws for 
application to a bank-to-turn missile. In the development 
of this work. Chapter 2 will describe the kinematics of the 
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missile and formulate the equations of the motion. All 
major assumptions are listed. The optimal controller is 
described in Chapter 3. The simulation results of the 
controller on three scenarios described later are used to 
check the suitability of the optimal controller. The 
optimal estimator is covered in Chapter 4, the formulation 
of the state equation, measurement equations and the 
derivations of the elements of Jacobean matrix are covered. 
The extended Kalman filter algorithm for non-linear system 
is then reviewed. Before the final computer simulation a 
discussion is given on the nominal parameters needed in 
initializing the Kalman filter. The results of the simula- 
tion of estimators with two -7 four-, and six-measurement 
vectors on a typical scenario are then presented. In 
Chapter 5, the simulation results of the control laws imple- 
mented the estimator with two-, four- and s ix -measurement 
sensors on three scenarios is described. The detail analy- 
sis of the results is developed in order to investigate the 
effect of the key variables of the control system on a bank- 
to-turn missile. The mean miss distance determined from 50 
Monti Carlo runs as a performance standard was used in the 
analysis of the results of the control laws as a function of 
the key variables. Finally, the conclusion are summarized 
in the last Chapter. 
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II. KINEMATICS OF A BANK-TO-TURN MISSILE 



A. ASSUMPTIONS 

The geometry and the equations of motion of the missile 
will be developed, representing the positions in space, 
under the following assumptions: 

1. The local geographic coordinate system will be used 
as the inertial reference. 

2. To simplify the equation of motion the stability 
axes are adopted for the missile body. 

3. The missile velocity due to thrust is constant. 
There is no missile acceleration in velocity due to 
thrust . 

4. Each control surface or surfaces is rigid. 

5. Relative to the body axes, each control system has 
only one degree of freedom. The missile's control 
acceleration vector acts normal to the velocity 
vector, that is, the dot product of acceleration 
vector and velocity vector is zero, also the 
acceleration acts through the missile center of 
gravi ty . 

6. The Euler angles <t> , 0 and <p will be used to 

V V 

describe the orientation of the missile with 
respect to inertial space, where <f> and 0 r are 
horizontal and vertical flight path angles, and <j> 
is the roll or bank angle. 
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7. The first order lags with time constants 



and 



l P 

t to a roll rate command P and a normal 
o c 

acceleration command a c will be considered for 
the dynamic response of the missile. 

B. MISSILE CONTROL METHOD 

Before going into the mathematical detail concerning 
the motion of a missile in space as a result of a guidance 
command, it is helpful to review with the control law for a 
bank-to-turn missile. The guidance system detects whether 
the missile is flying too much or too high to the left, 
right or vertical. The guidance system measures these 
deviations or errors and sends signals to the control system 
to reduce these errors to aero. The task of the control 
system therefore is to maneuver the missile quickly and 
efficiently as a result of these signals. In a bank-to-turn 
missile system, the guidance angular error detector produces 
two signals R and <j> in terms of polar coordinate expres- 
sion, showed in Figure 2.1. The same signals can be 
expressed in another way, that is, in cartesian coordinate. 
The usual method is to regard the <p signal as a command 
to roll through an angle a measured from the vertical 
and then to maneuver outwards by means of the missile's 
elevators. The method of maneuvering the missile is as 
follows. The <J> command goes as a positive command to one 
control surface and a negative command to the other, this 
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MISSILE 



I 




TARGET 



FIGURE 2.1. MISSILE CONTROL SYSTEM IN POLAR CO-ORD. 

causes the missile to roll. The R command goes to 
surfaces always as a positive demand, this causes the 
missile to accelerate normal to the velocity vector. The 
intention is to make the response in roll fast so that the 
commands can be applied simultaneously which makes for 
simplicity, only a pair of control surfaces are used as 
ailerons and elevators at the same time, control is obtained 
by means of the separated servos. 

C. GEOMETRY OF MOTION IN SPACE 

The motion of a missile as a particle may be described 
by using coordinate measured with moving axes (relative- 
motion analysis). The analysis of motion can be simplified 
by using measurements made with respect to a moving coordi- 
nate system. In present work, the equations of motion will 
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MISSILE 

LONGITUDINAL 

AXIS 



FIGURE 2.2 GEOMETRY OF RELATIVE MOTION. 
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be described with respect to the initial missile body axes 
taken as the inertial axes. After launching, the missile 
body axes changes position, but the relative position, 
velocity and acceleration are still computed in inertial 
axes, that is, in the initial body axes. The current zero 
effort miss distance is calculated. This will then be 
transformed to values in missile body axes to compute the 
amount of the control inputs to the missile using Euler's 
transformation . 

Figure 2.2 shows the missile and target as points with 
respective vector velocity V and . In this analysis, 

because [ V | is assumed to be at least 2 | V | , and the 

angle of attack is assumed to be small, the lead angle <£ 
between V R and the missile longitudinal axis is small. 

Let the vector denote the relative position of the target 
with respect to the missile. The orientation of the sight 
line vector in inertial space can be represented by the 
angle -j; R and G R , as shown in Figure 2.2. The sight 
line vector is resolved into three components in inertial 
axes as follows: 



Rx = 


R*cos (8 R ) *ccs (t> R ) = 


Xt 


- Xm 


(2.1) 


Ry = 


R*cos( 9 r ) *sinO R ) = 


Yt 


- Ym 


(2.2) 


Rz = 


- R*sin (<J> R ) 


Zt 


- Zm 


(2.3) 


Where R 


is the magnitude of 


the 


sight line vector. 
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Define the relative velocity vector: 



V = V' - V 

-r -t -m 



12 . 4 ) 



Where Y. and V are target and missile velocities, 
t in & 

The rate of change of the sight line vector's magnitude 
is equal to the component of along R . It can be 

expressed as following: 



R = 



! Y t R l 



R 



( 2 . 5 ) 



R = 



V *R + V * R + V *R 
rx x rv y rz 2 

? ? ' 2 

(R x + R “ + R_ ”) 

A \ L 



( 2 . 6 ) 



Where V = V - V 

rx tx mx 



V = V, - V 
rv tv mv 



V = V - V 
rz tz mz 



are the relative velocity components in the inertial frame. 

In the geometry, the relative angles and 0^ also 

will be expressed by the relative vectors: 



a R - - taA 1 



R 

z 

7 2 2 " " 1/2 

( R X + R y ) 



( 2 . 6 ) 
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tJj = -tan* 
r r 



R 

v 

R 



(2 . -) 



Where 6^ and are the elevation and the acimuth angles 

of the relative sight line vector in the inertial frame. 

Then the rate of change of both angles are 



- sin0 cos tl V - sin0 si ml V + cos0 V 

v r y YX v ^ t y*v \r 



V 



V rv 



v r: 



9 9 9 i 

(R v - + R v " + R_“) I/_ 



( 2 . 8 ) 



x 



% = 



rj 



Rcosf 



v 



-sinf V + cost V 
_ v rx ' v ry 

? 

R ", 

+ 2 ) COS 0 



9 9 

(R “ + R “ 
x v 



(2-9) 



v 



Where V no and V n , are the rates of change in Q and 
ip v components, is shown in Figure 2.3. These quantities 
will be used later in developing the estimator state 

equations and will also be used to represent the variables 

¥ * 

for the missile seeker. The quantities 6 r ’ e R ’ ^r ’ ^r ’ 
R and R (corrupted by noise) are the set of measurements 
assumed available from the missile seeker. 



D. KINEMATICS OF CONTROL VECTOR 

Figures 2.4 and 2.5 are views from near and side of 
missile. a m and <f> represent the magnitudes of the 
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K 




FIGURE 2.3 RE L AT I \’E VELOCITY COMPONENTS IN POLAR CO-ORD. 
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DEROLLED 

-2-AXIS 




DEROLLED Y-AXIS 



FIGURE 2.4 KINEMATICS OF CONTROL VECTORS VIEWED 
FROM REAR OF MISSILE. 




FIGURE 2.5 KINEMATICS OF CONTROL VECTORS VIEWED 
FROM SIDE OF MISSILE. 
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missile acceleration due to the acceleration command ^acj 
and the bank angle due to the roll rate command (Pc) each 
other. g represents the gravitational effect at the center 
of gravity of the missile. As mentioned in control method, 
the sign of a is such that it is positive upward with 
respect to the wings. Figure 2.5 shows that the accelera- 
tion normal to the velocity vector has the components due 
to the elevator control input and the effect of the gravity. 
The total magnitude of the acceleration normal to the 
velocity vector can be represented as 

a *cos<}> - g*sin6 
m ° v 

The velocity and acceleration components for simple 
circular motion can be represented as 

a n " V ’ /p 



See Reference 1. 

Using this definition, the differential equations 
describing the rate of change of the flight path angles are 

a cosp - geos 9 

= v “ (2 ‘ 10) 

m 
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( 2 . 11 ) 






v 




v 

v 

III 



s ini> 
cos 9 

v 



Note am*cosq - g*cos0 v is the magnitude of the normal 
acceleration component to the velocity vector in missile 
side plane and am*sin4> is the magnitude of the normal 
acceleration component to the velocity vector in the x-y 
plane. The acceleration component of the missile velocity 
V has only the component of the effect of gravity in the 
missile side plane under the assumption that the missile 
velocity is constant. The differential equation is: 



V = -g*sin6 
m ° v 



( 2 . 12 ) 



The rates of change of the missile position components 
(Xm, Ym, Zm) can be obtained by integrating the components 
of missile velocity. From Figure 2.2, the components of 



missile velocity (Ymx, Ymy , Ymt) in inertial 


frame are 


given by 




X = V = V *cos0 *cosiii 

m mx m v • v 


(2.13) 


Y = v = V *cos0 *sin>|i 

m my m v r v 


(2.14) 


7 = v _ = - V *sin0 

m mz m v 


(2.15) 



Let the response of the missile to input command in 
normal acceleration and roll rate be considered. If a 
demand is made on a missile for a transverse acceleration, 
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it is initiated by sending a signal to the appropriate con- 
trol surface. The missile acceleration a m follows the 
demanded acceleration a, in a manner which may be 



characterized by a frequency u> (weather cock frequency) 
and a damping factor P . For simplicity, in present 
work, only the first order lags with time constant and 

t q are assumed, the equation of the first-order system can 
be represented as follows: 



with zero initial condition. Applying these to the BTT 
missile cont rol sys tern. 



X(t) + 1 . /t *X(t) ) = f(t) 



Taking the Laplace transformation on has 



X(S) = F(S)/(S+1./T) 




a 



c 



(2.16) 



T *P + P = P 
P 



c 



(2.17) 



Rearranging the differential equation. 



a 



m 




(2.18) 



P = (P - P )/ T 
v c . m p 



(2.19) 



cj> = P 



( 2 . 20 ) 
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Where a and P 
■ c 



are the acceleration and roll rate 



c 



commands. Equation (2.20) is by definition. From the 
above differential equations the control ratios are 



transformed as follows: 



a (S) 
m 



1 



( 2 . 21 ) 



3^s7 




P IS) T S + 1 

C J p 



P (S) = 1 



( 2 . 22 ) 



Equations (2.10) through (2.22) constitutes the dynamic 
equations describing missile motion in response to the 
command inputs ac and Pc . The solution of these 
equations provide missile position (Xm , Ym , Zm) , 
orientation and magnitude of velocity (if ,0 , Ym) and 
orientation and magnitude of control acceleration ( cj> , am) . 

E. CO-ORDINATE TRANSFORMATION 

Given that the missile is on guided flight to compute 
the magnitudes of the control inputs at any moment, it is 
necessary to transform the instantaneous values from the 
sensors in the inertial frame to the instantaneous values in 
body frame. The transformation from one frame to another 
can be accomplished through a transformation matrix. The 
transformation matrix wiill be developed. 

Define an inertial coordinate system with unit vectors 
I, J, K. Also, define a missile body axis coordinate system 
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with unit vectors i, j, k. It is desired to find the trans- 
formation between the I, J, K system and the i, j, k system. 

The I, J, K system can be thought to be oriented so 
that I points north, J east, and K down. Similarly, the 
i, j, k system has i along the longitudinal axis of the 
missile (which by assumption is along the velocity vector 
Ym) , j out the right wing, and k down. 

Consider three successive rotations p , 0 and i> . 
The rotation is about the inertial z axis, and 

transforms to an intermediate axis systems i^, j ^ , k ^ . 

The 0 rotation is about the j, axis and transforms to an 
axis system i ? , j ? , k 7 . The last rotation <j> about the 
i ? axis transforms from flight path axes to missile body 
axes. As a result, the total transformation from inertial 
to body axes is given by 



i 




i 




- — 

I 


i 

k 


= [$][e v ]|> v ] 


j 

K 


= [A] 


J 

K 



( 2 . 23 ) 



Where A is the total transformation matrix from inertial to 
body frame. The elements of the a matrix from Reference 1 
are : 

all = cos ( 0 ) *cos (^ y ) 

al2 = cos ( 0 v ) * sin (ty ) 

al5 = - s in ( 0 ) 
v' 
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a21 = sin(-,J *sin(3 v ) *cos ty v ) - cos(;j *sin( , 



a2 2 


- s i n ( o) s i n ( _ J * a i n ( j 
\ \ 


+ COS ( y) " COS ( j j 


a2 5 


- s i n ( . 0 ) * c o ( 9 ) 

V 
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= COS (f) - s i n ( 9 y ) *cos O y ) 


+ sin(.$) *sin(^ v ) 
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= cos(?) s i n ( 6 v J *sinU v ) 


- sin(t) *cos(v y ) 


a55 


= cos ( $) "COS (. 6^ J 
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III. AN OPTIMAL CONTROLLER 



Although this work is primarily concerned with the 
effect of an optimal estimator on the performance of a bank 
to- turn missile, we will first outline the theory of an 
optimal controller in this section. The guidance laws 
considered here were first developed by Stallard [Ref. 10]. 
The following is a brief outline of his work. 

A. GEOMETRY 

Figure 5.1 shows the applicable geometry representing 
the Projected-Zero-Control miss (PZC miss) distance in the 
terminal state. The problem is considered to be three- 
dimensional and two major axes system are used: 

1. A seeker-oriented axis system for estimation or 
measurement, and 

2. The three principal axes of the missile for the 
control problem, 

Xb , Yb , Zb denote the target coordinates relative to the 
missile along its principal axes. The body system is repre 
sented a time T = 0 by a set of Eulian angles which 
relate it to an earth fixed system. The angle A<p is the 
incremental roll angle from the present missile axes to any 
future orientation at time T , shown dashed in Figure 5.1. 
The PZC miss represents the total Proj ected- Zero effort - 
miss distance and is defined as the miss distance which 
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would occur if no further control was exercised on the 
missile at I = Ti . It is composed of a Y- components 
(PZCY miss) and a I-component (PZCZ miss). Under the 
assumption of constant velocity the X-component (PZCX miss) 
is represented by the distance that the missile has to fly 
at any time, since it is not effected by control, it is 
not shown in Figure 5.1. 



7 




FIGURE 5.1 COMPONENTS OF PZC, MISS DISTANCE. 
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B. SUMMARIZED DESCRIPTION OF AN OPTIMAL CONTROLLER 



Given a time- varying linear system of the form, 

X = F(t) X + G ( t ) U (3.1) 



Where X is n-component state vector 

U is m-component control vector. 

An optimal control is one that minimizes a performance 
index made up of a quadratic form in the terminal state 
plus an integral form of quadratic forms in the control and 
the state: 



J = j (x T s f x T 



T i^ + 2 



T. 

1 



(X T AX 



U r BU) dt 



(3.2) 



Where the S £ and A are positive semidefinite matrix and 
B is a positive definite matrix. An appropriate choice of 
these matrices must be made to obtain acceptable level of 
X(Ti) , (X(t) and U(t) . 

The guidance law determined in Reference 8 is of the 
form : 



U = 
c 




P c) 



T 



With the performance index chosen (the weighting on the 
state vector was not considered in Reference 8. 
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J = I (X b + Y b - + : b -)| Ti 



Ti 



(U * U b ) T B(U - u b )dt 



T 



(3.5) 



Where the first term is one-half the required miss distance, 
the second term is included in order to eliminate infinite 
energy solutions. B is a 2 by 2 positive definite, 
symmetric weighting matrix and of the form 



B 



B1 0 
0 B2 



(3.4) 



with 1/Bjj = (Ti - To)X maximum acceptable value of 
|U b (T) “ . A detailed derivative of the optimal guidance 

law is given in Reference 10. 

The solutions of the above equation from Reference 8 
give the optimal acceleration command at time T between 
TO and Ti as 



3T 



(- M bz ) 



3B , + T ' 
1 go 



(3.5) 



Where T is the remaining time to go until intercept and 

C> 

M, is the PZC-miss along the missile normal force axis and 
b z ° 

the negative sign is due to definition of the normal 
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acceleration as positive in the upward direction. The 
optimal roll rate command at time T between TO and i'i 
from Reference 8 is given by 



P 



c 



21a T 
c go 



2 T 

a T ^ 
c go 



( M bA 

6 3 B 0 



(3.6) 



Where M^ v is the PAC-miss along the wing axis . Defining 
the roll error: 

1 Mu 

a , _ 1| by i , _ 

A<p = tan lvj-^-1 (o.7) 

b z 



.An alternate form for P can be obtained bv dividing 

c - ° 

Equation (3.6) by Equation (3.5), then 



/ a 



T (T ' 
go go 



3 B x ) 



7 

a " T 



go 



+ 63 B, 



tan (A<j>) 



(3.8) 



Because the solution of P c was based on an assumption of 
small missile roll excusions, the tanA$ term in 
Equation (3.8) can be replaced by A <Jj to yield 



P 

c 



7a " T T J + 3 B.) 
c go _go 1_ 

a 2 T 5 + 65 B, 
c go 2 



A<J) 



(3.9) 
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The optimal control lei* is implemented in the present work 
using Equations (5.5), (5.7), and (5.9). In computing the 
control input, T (tq , Y and Z-component of PZC-miss and 
the elements of the weighting matrix are needed. 

C. APPROXIMATING TIME -TO- GO 

It is necessary for the Time-To-Go and the Projected- 
Mi ss - Dis tance in missile body axes to be specified at time 
T to compute the optimum control input. The Time-To-Go 
will be defined at first, then the state variables will be 
introduced as means for formulating the necessary equations. 
The target acceleration components of the state variables 
will be modeled since it is assumed that any information 
about the target acceleration cannot be obtained from the 
active seeker of the missile. Then using this target 
acceleration model the algorithm for computing the other 
state variables will be developed. 

The Time-To-Go until intercept, T^ , is required for 
computing the control input. Let the time to intercept be 
defined as the time to minimum range. Under the assumption 
that the relative velocity vectors may be considered 
constant at its present value until intercept. T can be 
found at time, T , as: 



T 



T. 



1 



(5.10) 
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V 
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Where R is the present position of the target relative to 
the missile and the factor in brackets in the component of 
V along R . An alternate form of T is: 



T. 

1 



R V + R V 
x rx v ry 



.2 ,.2 

+ Y + 
rx ry 



+ R,V 





(3.11) 



The negative sign comes from the sign of V as shown in 

O O O 

Figure 2.2 and Equation (2.6). 



D. PROJECTED-ZERO-COXTROL MISS (PZC MISS) DISTANCE 

The Pro j ected- Zero -Control miss distance (PZC-miss) is 
defined as the miss distance which would occur if there 
were no further control input after time T . 

In this work the state vectors are defined as the 
following state variables 



X 



= 



R , 

y 



R 



V , 
rx 



V , 

ry 



V 



tx 



ty 



a tz) 



Where R is the relative missile distance to target 

V r is the relative missile velocity to target 
a^ is the target acceleration 
A model for target acceleration has been assumed. This 
is given as follows: 



a t ( T ) = a t o e * a ' T ' a 1 0 



(3.12) 
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the 



Where a is the intial target acceleration and a is 
reciprocal of the acceleration time constant. The initial 
target acceleration, 4g's, will be assumed for each 
scenario, and a = 1/20 for evasive maneuver as indicated 
in Reference 10 will be chosen. These values will be used 
in the simulation of the optimal controller. 

With the assumption that a state estimator is available 
to provide estimates of the state vector at current time (t) , 
the values of the state variables for no control inputs at 
future time, can be obtained easily using the integration, 



1 . e. , 




to 



R(t) = R(t0) +J V (x)dT 

-4- n 4. 



to 



The results of these integrations are 



V r (t) = V r (t0) + 1/a (1 - e ‘ a(t ' t0 ^ ) a t (tO) (5.15) 



R(t) = R(t0) + (t - tO)*V r (to) 



(5.14) 




Where tO and t denote current time and future time. It 
should be noticed that above integration is based on the 



assumption of a single physical dimension. If this assump- 
tion is to be applied to three-dimensional problem, it must 
be concluded that there are no coupling among the X, Y, I 

dimension. Let T = t - t , then the components of the 

go o 1 

PZC-miss in inertial axes at current time (tO) can be 
expressed as 
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Where f = f = f = 1/a [aT - 1 + e go] and the last 
x y z L go ® J 

term accounts for the free fall effect of gravity in the 
vertical axis . 

The components of the PZC-miss in missile body axes for 
control input calculation are obtained via the Euler trans- 
formation matrix: 
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l3.lt>) 
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bx 
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L z J 



Where it is assumed that the missile has exact knowledge of 
its orientation angles ip , 9 . , d , from the accurate 

V \ 

inertial measurement units. 

The values of optimal control vectors (a c and P ) , 

then, are calculated from time to go (T ) , PZC-miss in 

’ ° v dO 

o 

body axes and weighting matrix components (B1 and B2) which 
may be determined according to the desired miss distance and 
intercepting time. In a later section, the weighting 
matrix will be developed and the scenarios will be set up 
for computer simulation. 

E. SIMULATION RESULTS 

The guidance law in this work was tested on a simple 
six-degree-of- freedom simulation using only plausible 
numbers without reference to any actual missile or design. 

Before the computer simulations, the weighting matrix 
of the performance index selected in Reference 8 will be 
calculated using the initial state, X(0) , the missile 

operating time imposed, T (5 sec) and the constraint input 
vectors. It is assumed that the missile has a velocity 
advantage of two over that of the target, a zero- and 0.5 
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second-time lag auto pilots for pitch with limits of - 3g ’ s , 
+20g's and for roll rate with limits of +6.23 rad/sec. 

The allowable miss distance which is usually determined 
by sizing of the warhead was chosen as 5 meters. Which 
makes the first term of the performance index Equation 
(3.3) equal to 12.5 meters. The acceleration term and the 
roll- rate term in the performance index are set equal to 
this respectively (one-half of square mean miss distance), 
then 

B 1 f i ? ? 

— J (a c - a d ) “ dt = 12 . 5 m“ (3.16) 

0 




? 

dt = 12.5 m“ 



0 



(5.17) 



The nominal time of flight for the terminal phase is set 

equal to 5 seconds. The components of the weighting 

matrix B were based on the following assumptions. It is 

? 

assumed for (4g's)“as a mean-square value of (a c - a^) and 
? 

(2 rad/sec) “ as a mean-square roll rate to be acceptable. 
Then the components of weighting matrix are: 

B 1 = 0.003254 sec 0 

B = 1.25 m“s ec 
? 
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The commands a and P are computed every 0.05 sec 

and are frozen when the Time-To-Go falls below this interval. 

In order to simulate the optimal controller appropriate 
scenarios were developed below with restriction that the 
Time-To-Go was 5 sec and the speed of missile is two times 
faster than that of target. Three scenarios; tail-chase, 
head-on, s ide- approach engagements, were chosen as possible 
cases in this work. 

In case 1, tail-chase engagement, the target flies 100m 
above the missile and 2500m uprange with a horizontal east- 
ward acceleration of 4g's. Figure 3.3 shows the case 2. 

In case 3, s ide - approach engagement, the target flies 400m 
above the missile and 1900m uprange with a horizontal north- 
ward acceleration of 4g's at t = 0 as shown in Figure 3.4. 
For simplicity, when the scenarios were set up, lOOOm/’s was 
taken as a missile speed, 500m/s as a target speed in case 
1 and case 2, 600m/s as a missile speed, 300m/s as a target 
speed in case 3, and 4g's as a target acceleration for all 
three cases. When simulation was executing, this first 
simulation assumed perfect state feedback without the use of 
an optimal estimator. 

The simulation results are shown in Figure 3.5 through 
Figure 3.10. At first, the optimal controller with no time 
lag was tested for three scenarios, as shown in Figures 5.5 
and 3.6. For scenario 1, the normal acceleration command 
in initial phase is very large, then decreases to zero at 
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FIGURE 3.2 SCENARIO-1 TAIL-CHASE ENGAGEMENT. 
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FIGURE 5.3 SCENARIO- 2 HEAD-ON ENGAGEMENT. 
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/ 



FIGURE 3.4 SCENARIO-3 SI DE- APPROACH ENGAGEMENT. 
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FIGURE 3-6. ROLL RRTE COMMAND VS TIME 
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FIGURE 3-7. MISSILE BRNK ANGLE VS TIME 
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at the final time. For scenario 2, the acceleration is zero 
in the initial phase, then increases and converges to zero 
finally. For scenario 3, the acceleration starts from zero, 
but, increases rapidly up to the maximum acceleration then 
freezes for seconds before decreasing. The acceleration 
does not converges to zero at the final time. Comparing 
these results with the variation of the roll rate command 
in Figure 3.6, the opposite trends are observed in the 
results of scenarios 1 and 2, i.e., if an acceleration 
command in the initial phase is large, a roll rate command 
is small. The acceleration command in Equation (5.5) is a 
linear function of PZC-miss in of z-direction of the missile 
coordinate system. For scenario 1, PAC-miss in the 
z-direction is relatively large, this causes a large accel- 
eration command and a small roll rate command in initial 
phase. For scenario 2, the relative magnitude of the PZC- 
miss in the z-direction is small. This cause a large roll 
rate command in the intial phase. For scenario 3, the 
PZC-miss in y-direction is of the same magnitude as the 
relative distance as the Time-To-Go decreases. This means 
that a larger acceleration command than the maximum value 
allowable in the missile body coordinate system is required 
at some of instance time during the missile flying. Thus 
the acceleration does not converge to zero at the final time. 
In the derivation of the equations for control commands, a 
small angle approximation was assumed. However, in the 
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scenario 5 this assumption is questionable. The roll rate 
command computed as shown in Figure 3.6 shows a sine wave 
characteris tics which is possibly due to overcorrection . 

The simulation results for a 0.5 second time lage are 
shown in Figure 3.8 and Figure 5.9. The time lag causes 
much larger acceleration control commands. This includes 
overcorrections, so that the control commands do not 
converge in the final phase. The other characteristics 
follow the explanations given in the analysis of the cases 
with no time lag. 

For all three scenarios the miss distances were obtained 
in the simulation of both cases, no- and 0.5 second-time 
lag. The miss distances are below 0.5m for all three 
scenarios in the case of no-lag. In the no lag case the 
control laws gave excellent results. In the lag case, the 
miss distances are 5.9m for tail-chase engagement, 0.8m for 
head-on engagement, and 32.9m for s ide- approach engagement. 
The miss distance of the s ide - approach engagement with a 
different maximum acceleration limit fo 23g's results in a 
miss distance of less than 5m. This higher maximum acceler- 
ation limit will be used in the scenario 3 during the simula- 
tions for testing the performances of whole control system 
in Chapter 5. 

The previous results are summarized as follows: (1) the 
tracking of a target is very dependent of the missile and 
target geometry, (2) the capabilities of the missile 
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(maximum acceleartion and maximum roll rate limits and time 
constant) in maneuvering against the target is very important 
factors, (3) higher maximum accelerations are required in 
any side approach engagement, and (4) the assumption of 
small A>t> in the optimal control law is a limitation in the 
general case. Nevertheless, the control laws developed 
from the optimal control theory for both the no lag and the 
0.5 second-time lag cases yielded successful results for 
the specified scenarios. Since the optimal controller with 
0.5 second-time lage auto pilots resulted in a miss distance 
of approximately 5m with complete state variables feedback, 
it is expected that the inclusion of an optimal estimator for 
these particular scenarios will results in still larger miss 
dis tances . 
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IV. OPTIMAL ESTIMATOR 



A. DESCRIPTION 

Without full state variable feedback it is necessary to 
estimate the state variables. This chapter describes the 
Kalman filters used in the state estimation. These filters 
were designed for respectively: (a) two measured states, 

(b) four measured states, and (c) six measured states. 

All measurements have noise superimposed on them. It is 
assumed that the nature of the noise source is known 
completely . 

The Kalman filter is the optimal estimator of the 
states. Since the measurements are non-linear, it was 
necessary to use the extended Kalman filter theory. A 
first order extended filter was assumed in this work. In 
developing the filter algorithm, the non-linear measurement 
equation is linearised about the most recent state estimate 
and then the Kalman filter algorithm is applied. This is a 
extended Kalman filter algorithm. The results of the 
extended Kalman filter will yield suboptimal state estima- 
tion. 

In practice, the missile will have sensors of various 
types that provide inputs which are related in some fashion 
to the states to be estimated. The main emphasis in the 
present work will be on active seeker which is assumed to 
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provide the optimal estimator with measurements of line-of- 
sight angles for the case of the missile having two measure- 
ment sensors. For the case of four measurements one has two 
of line-of-sight angles, range and time rate change of 
range. For the case of six -measurements one has two of 
line-of-sight angles, two of time rate change of the line- 
of-sight angles, range and its time rate change. When the 
measurement equations and the Jacobean matrix are developed 
the six -measurement case will be derived since the other 
cases have the same kind but less number of measurement 
sensors . 

We will first consider the state equations and then the 
measurement equations. This will be followed by a discus- 
sion of the extended Kalman filter. Measurement error 
effects on the initialization of the Kalman filter will be 
analyzed. Finally, the estimator for the measurement cases 
will be simulated in order to test the effects of the imple- 
mentation of the different measurment vectors on the 
extended Kalman filter. 



B. STATE EQUATIONS 

The state vector is limited for convenience to the 
following state variables: 
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( 4 . 1 ) 
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Where X^ = R^, X., = R , X_ = R_ are the components of 

relative position, X. = V , X r = V , X. = V the com- 
1 ’4 rx 5 ry o rz 

ponents of relative velocity, and X,_ = „ , X Q = a , 

Xg = di the components of target acceleration. 

In order to develop the state equations, dynamic equa- 
tions of target motion are needed. The target model 
selected for tracking applications must be sufficiently 
simple to permit ready implementation in weapons system for 
which computation time is at a premium yet sufficiently 
sophisticated to provide satisfactory tracking accuracy. 

To meet these requirements, the model described by Singer 
[Ref. 10] will be used in this work. The model will be 
presented for a single spatial dimension in order to 
enable accurate tracking performance estimates to be made 
for a variety of sensor measurement. If the targets under 
consideration normally move at constant velocity, the 
accelerations due to turns or evasive maneuvers may be 
viewed as perturbations upon the constant velocity trajec- 
tory. The target acceleration a(t) therefore will be 
termed the target maneuver variable, in a single physical 
dimension. The target maneuver capability can be satis- 
factorily specified by two quantities: the variance or 
magnitude of the target maneuver and the time constant, 
or duration, of the target maneuver. Hence the target 
acceleration, namely, the target maneuver is correlated 
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in time. A typical representative model of the target 
acceleration as a correlation function, can be expressed 
as follows: 

a t (r) = E[a t (t)a t (t + t)] = o ffl 'e a > 0 (4.2) 

? 

Where the o is the variance of the target acceleration 
m ° 

and a is the reciprocal of the maneuver time constant. 

? 

The variance c m “ of the model will be approximated using 
the following formula which represents the acceleration 
probability density suggested by Singer [Ref. 10]: 



o 



? 



m 



max 



[1 



+ 4P - PJ 
max 0 



(4.3) 



where a is a maximum acceleration rate, P is the 

max ’ max 

probability of maneuvering at a , and P A is the 
probability of no target maneuvering. And as mention in 
previous chapter, a = 1/20 for an evasive maneuver will 
be used in this work. 

The above process can be modeled as the output of a low 
pass filter driven by white noise and it can be described 
by the equation 

a t = - a + W (4.4) 
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where 



c ”(t) , the correlation function of the white noise 

w 

input satisfies 

a w “(i) = 2aa ni “o(T) (4.5) 

The acceleration model above will be applied to each 
X, Y, Z dimension with no cross coupling, the system state 
equations can be written then as: 
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(4.7) 

The Equation (4.7) is a linear state equation with the 
assumption that the forcing function a vector, i.e., missile 
acceleration, can be precisely measured and also resolved 
into X, Y, Z components in the inertial frame. 
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Many sensors have a constant data rate, sampling the 
data every T seconds. If the above system equation is 
represented in discrete equations. The appropriate system 
equations of motion may be expressed by 



X(K + 1) = 0(T)X(K) + B ( K) + b(K) (4.8) 

where' o(T) is the target state transition matrix, B(K) 
the deterministic forcing input vector which is composed of 
first and second integration and U(K) the inhomogeneous 
driving input due to white noise. Since it is assumed that 
the missile acceleration is a known deterministic forcing 
function, the system equation of motion can be obtained by 
direct integration in each single dimension with E|W(t)|=0. 
The desired integration in discrete form yields 
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X(K + 1) = $ (t , a ) X (K) + a (K) (4-9) 

Where K + 1 = (K + 1)T = t + T, K = KT = tO 
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Next, if the driving input Uft) vector is considereu, 
this input is not a sampled version of the continuous tine 
white noise input W(tj . Since W(t) is white noise, 

U(k) also should be a discrete time white noise sequence, 
that is, E [U(K) U(K+i) ] = 0 for i f 0 . Then as shown 
in Reference 10, U and W are related by 

, (K+l) 

U(k) - J $| [K + l)t - x | W(t)dt (4.10) 

KT 

Where U(K) is a white noise sequence with covariance 
mat rix 

E[U(K)U(K) T ] = Q (4.11) 



Following the above derivation, it can be easily 
expanded to the case of three independent dimensions in X , 
Y, Z. For a = a y = a. = a , and = a, = a 

since the model of target acceleration is for one dimension, 
the transition matrix is 



$ 



I TI f x I 



I f ? I 



(4.12) 






0 



0 



Where I and 0 are 3 by 3 identify and null matricf 

f. 



l/a“ (aT - 1 + e" aT j 



1 



f 7 = l/a(l - e" aT ) 



f- = e 
;> 



-aT 



The forcing acceleration vector is 
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B 9 

! 


1 



- ff 



(K + 1) T 
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4.15) 
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The covariance matrix of white noise b(IO’ is 



Where 



‘11 



(a"/a 4 ) [1 - e 



ll 1 


Q12 1 


QlS 1 




12 1 


Q 7 n I 


Q23 1 


( 4 . 14) 


h- 4 
04 
1 — 1 


Q 23 1 


Q33 1 




- 2aT 
e 


+ 2aT + 


(2/3) (aT) ° - 2 (aT) 2 



4aTe * aT ' 



Q 12 = (o7a J ) [e’ 2aT + 1 - 2e ~ aT + 2aTe~ aT - 2aT + (aT) 2 ] 



07 - ?nT -nT 

Q i3 = (a“/a“) [1 - e “ al - 2aTe al ] 



? 2 - ft T - ? n T 

Q 2? = (a /a“) [4e - 5 - e + 2aT] 



n r 2 , w - 2aT , , -aT, 

Q 23 = (a /a) [e + 1 - 2e ] 



7 - 7 rv T 

Q 33 = o-[l - e “ aT ] 



C. MEASUREMENT EQUATION 

In the case of the estimator with six -measurement sen- 
sors the present study assumes that the active seekers of 
missile give the measurements of the sightline angles 
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6^ , i]^ , the time rate change of the sight line angles 
Q u , T n , the relative range R , and the time rate change 
of the relative range R contaminated by the Gaussian white 
noise. Then the equations of measurements in discrete time 
form are 



Z(K) = h(x , K) + V(K) 



(4.15) 



where ^Z(K) is a sequence of measurement vector contaminated 
by white noise V(K) and 



h(x) 



elevation angle 






elevation angle rate 




0 

' r 


azimuth angle 








- 


r r 


azimuth angle rate 




P ^ 
± 


range 




R 


range rate 




R 



The missile seekers actually measure the data about the 
seeker axes. Thus, throughout this development the assump- 
tion is made that the missile possesses an inertial measure- 
ment unit that accurately specifies the missile's orienta- 
tion in space so that a transformation from seeker to 
inertial coordinates can be made. 

Referring to Figures 2.2 and 2.5, the six-measurement 
vectors can be written 
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e R = - - tan 
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The measurement vector can be expressed in terms of state 



vector 



h(x) = 



h. 
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tan 
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X. 
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(4.16) 



The measurement noise covariance matrix for the active 
seeker can be written 

2 



E[V(K)V(K) T ] = R = 
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0 0 0 0 
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0 0 0 
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a, 0 0 

’P 

o o o a ; 2 o o 

0 0 0 0 a R 2 0 
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.2 

R 



(4.17) 



Where the diagonal elements are the variances of the indi- 
vidual measured quantities. 

As shown in Equation (4.16), the measurement vectors are 
the non-linear functions of state vectors. It is impractical 
to implement in non-linear form because the computation of 
gain K and error covariance matrix P in update process 
in extended Kalman filter algorithm is not possible. To 
simplify this computation, and to implement the extended 
Kalman filter, the Jacobean or matrix of partial derivatives 
H will be determined, i.e., 





(4.18) 



The Jacobean will be a 6 by 9 matrix because h is a vector 
with six elements and X is a vector with nine elements. 
Performing the operation, the components of the matrix H 
are 
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D. THE EXTENDED KALMAN FILTER 

Optimal estimators that minimize the estimation error, 
can be divided into two processes, the one is the updating 
process to estimate the state vector at the current time, 
based upon all past measurements, the other is the extra- 
polating process to estimate the state at some future time. 
Optimal estimation procedure for linear state equations and 
non-linear measurement in the form of discrete time will be 
described referring to Reference 6. Figure 4.1, a timing 
diagram shows the flow of the various quantities involved 
in the discrete optimal filter equations. 

The discrete system equation whose state at time kt is 
denoted by simply X(K) , where U(K) is a zero mean, 
white sequence of covariance Q ( K ) , is 

X(K) = $ X(K - 1) , + B (K - 1) + U(K - 1) (4.21) 

where 4 is a transition matrix 

E[U(K)] = 0 (4.22) 

E[U(i)U(j) T ] = Q5 — (4.23) 

The measurements are the non-linear function of the system 
state variables, corrupted by uncorrelated white noise of 
covariance R(K) . The measurement equation is written as 
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■ 4.24; 



ZOO = h (X,K) + Y( K) 
where E[U(K)] =0 (4.25) 

E[V(i)V(j) T ] = Ro i . (4.26) 



The initial conditions are 

E [ X ( 0 ) ] = X(0) E [ ( X C 0 ) - X Q )(X(0) - Xq)] = P Q (4.27) 

E(^kVj^) = ® uncorrelated (4.2S) 

The Kalman filter algorithm is to minimice the estima- 

/\ 

tion error, i.e., error covariance. If X denotes an esti- 
mate of state vector, X , and X is the mean value of the 
state vector, the error covariance matrix is defined as 

P = E [ ( X - X) (X - X) T ] (symmetric) (4.29) 

The development of the extended Kalman filter algorithm with 
a few definitions is essentially more application of the 
expectation operator E. We will consider the updating 
process then the extrapolating process. The update equa- 
tions are used to incorprate the latest measurement in the 
estimate and in the covariance. After we obtain the updated 
covariance matrix we will then consider the optimum choice 
of Kalman gain and derive the equation for the esti- 
mation of state. Let X(K-l) (-) denote an estimate of X 
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FIGURE 4.1 DISCRETE EXTENDED KALMAN FILTER TIMING DIAGRAM. 
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at time k-1 , given measurements up to and including ri e 
K-2 , P(K-1)(-) denote the error covariance, K ( K - 1 : 
denote Kalman gain and H(k-l) denote the Jaccobean matrix 
at time K-1 . We will seek an optimal observer in the 
presence of the state excitation noise and in the presence 
of the observation noise. 



P(K-l) (.+) = e{[I - K(K-1)H(K-1) ]X(K-1) [X(K-l) 1 (-) (I-K 
(K-l)H(K- 1) T ) + Y(K-1) T K(K-1) T ] 

(4 . 30) 

+ K(K-l) Y(K-l) [X(K-l) (-) T (I-K(K-1)H(K-1) T ) 

+ Y(K- 1) T K (K- 1) T ]| 

Rearranging Equation (4.50) using the definitions 
Equations (4.26) and (4.29) and the assumption that the 
measurement errors are uncorrelated, then one has 



P(K-l) (+) 



[I-K (K- 1)H (K- 1) ]P (K - 1) (-) [I - K(K-1)H(K-1) T ] 



K(K-l) R(K-l) K(K-l) 1 



(4.31) 



This equation updates the error covariance matrix. 

Then we still need an optimal gain K. We will choose to 
minimize the diagonal elements of the covariance matrix, 
i .e . , 

J(K-l) = E[X(K-1) (+) T X(K-1) (+) ] = trace [P(K-l) ( + ) ] (4.32) 
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mathematical model 




FIGURE 4.2 



SYSTEM MODEL AMD DISCRETE KALMAN FILTER. 
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Introducing a bit of matrix calculus, one has 



[trace ABA] = 2AB 
aA 



( 4 . 35 ) 



aK (K- 1) 



[I -K (K - 1) H (K- 1) ] = -H(K-l) 



(4.34) 



Applying these formulas to Equation (4.51) and solving 
for K(K-l) , 

K (K- 1) - P(K-l) (-)H(K-1) T [H(K- 1) P(K-l) (-)H(K-1) T + R(K-1) j" 1 



(4.35) 



This is the Kalman Gain Matrix. 

Rearranging Equation (4.31) 

P(K-1)( + ) = [l-KCK-l)H(K-l) ]P(K-1) (-1) 



(4.36) 



Then the update state estimate at time K-l is equal to 
the extrapolated state estimate at time K-2 plus a term 
which weights the measurement residual via gain K(K-l) . 

i(K-l )( + ) = X(K-1)(-) + K(K-l) [Z(K-l) -h(k-l) /x(K-l) (-)) ] 

(4.37) 

We need to consider also the propagation of the estimate 
X ( + ) between measurements and the propagation of the 
covariance matrix P(K-1)(+) between measurements. Propa- 
gation of the estimate is straight forward and involves only 
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FIGURE 4.5 DISCRETE KALMAN FILTER INFORMATION FLOW DIAGRAM. 
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the application of the state transition matrix and the 
forcing terms . 

x'(K) ( -) = $(K-l)£(K-lj (+) + U(K-l) 14.58) 

By the definition (4.29), solving for P(K)(-J 

P(K)(-) - <±>(K-1)P(K-1) ( + )$(K-1) T + Q(K-l) (4.59) 

Figure 4.2 illustrates the above developed equations in 
block diagram form which contains a system model, measure- 
ment process and the Kalman filter to be implemented, and 
Figure 4.5 shows a simplified computer flow diagram of the 
discrete Kalman filter. 

Equations (4.55) through (4.59) constitute the recursive 
formulas for implementing the extended Kalman filter 
algorithm. The process is initialicing by providing values 
x(0)(-) and p(0)(-) . In order for the extended Kalman 

filter to be simulated, the next section will describe the 
initialization of the Kalman filter. 

E. INITIALIZATION OF THE KALMAN FILTER 

The one objective of this work is to test the perform- 
ance of BTT missile control system having a Kalman filter as 
an optimal estimator as a function of different measurement 
vectors. All values for initialization of former worker in 
this work (Ref. 14) will be used for computer simulation. 
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1 . I nitializing the Error Covari ance Matri x , P 

Let a , c and o define the standard deviation 
p v a 

of relative position estimate, of relative velocity esti- 
mate, and of target acceleration estimate. The initial 
values of error covariance matrix p , will be chosen as 
follows. The first six elements of the initial state 
vector are determined by 

X(0) = x(0) + V(0) 

/"• 

Where the measurement vector X is the true measurement 
vector X plus V caused by measurement uncertainties. 

The expected mean square of initial covariance matrix 
components. With the assumption that the initial 
covariance matrix is diagonal with the diagonal elements 
reflecting the uncertainties associated with the initial 
state vector estimates, the components of the covariance 
matrix are 

H [ \^ 2 3 = e[y 2 2 ] = E [ V 5 2 3 = G p 

E[Y 4 2 ] = E[Y 5 2 ] = E[Y 6 2 ] = a v 2 

The last three elements of the state vector, acceleration 
components, are set to zeros 

X ? = X g = X 9 = 



0 



2. I nitializing the Measurement X o i s e Variance Matrix R 
Measurement errors for the active seeker arc- 
assumed to be due to thermal noise, gimbal angle pickoff 
error, environmental noise, and glint. The values of one 
sigma errors in each measurement will be used in present 
study. Because in the case of angle measurement, the envi- 
ronmental noise is proportional to the square of range to 
target and glint errors the wander of the apparent target 
centroid as seen by the seeker as varing inversely with 
range to target. The errors caused by noise are generally 
a highly complex function of the target geometry, target 
radar cross section, radar receiver, signal-to-noise ratio 
and etc. For simplicity, in present study, the constant one 
sigma errors (average values) in each measured vector are 
assumed to initialize the Kalman filter. Referring to 
Reference 15, the one sigma values of the average errors 
for measurement of angle, angle rate, range and range rate 
of the present active seeker are tabulated below. 

(1) angle measurement errors : 0.15 -0.6 deg 

(2) angle rate measurement errors : 0.5 -2.0 deg/sec 

(5) range measurement errors : 3 meters 

(4) range rate measurement errors : 6 m/sec 

Before the simulation runs is undertaken, it will be 
briefly discussed the manner in which the Kalman filter on 
board the missile is initialized at the start of the 
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engagement based on sensor data from the launch aircraft. 

The launch aircraft's sensors are assumed capable of deter- 
mining, within some prescribed accuracy, the relative 
position and relative velocity of the target with respect 
to the missile at the instant of launch. This information 
is sued to initialize the first six elements of the filter's 
state vector. However, the launch aircraft is assumed to 
be able to provide no information regarding target 
acceleration. Thus the three elements of the initial state 
vector are set to zero, finally, the complete list of 
nominal parameters for initialization is provided in Table 
4.1. 

F. EVALUATION OF THE OPTIMAL ESTIMATOR PERFORMANCE 
The performance of the state estimator with two- 
measurement vectors (case 1) , with four-measurement vectors 
(case 2), and with six-measurement vectors (case 3) , was 
simulated to evaluate the effect of the possible implementa- 
tion of the more measurement sensors on the bank-to-turn 
missile for typical scenario (tail chase engagement case). 
The error covariance and the Kalman gain components 
selected, and the states estimated, were computed as shown 
in Figure 4.4 through Figure 4.30. The control laws imple- 
mented with the estimator also were tested to evaluate the 
performance of the control system of the BTT missile as 
shown in Figures 4.31 through 4.35. 



30 






TABLE 4-1 



NOMINAL PARAMETERS 



* ; ;c:: 

1 jet: 

2 or* 5 



.. n - • * | 



a / .a r c _ 



a 6' ^ 



3 . ' 5 



3 cr.r *i?s? -Wl' Gg c- 



J . 5 ;;:A:C 



4 c: - 3 1 i y a s a a r. r 
rar. g - 

races rat.-. ^ r r c 

6 .a i s 3 1 1 -a r cl I :a“r 

c:r.s:a: 

7 missil-- rare a I v:c. 

*:im~ cm?* a:v 
raxi a uo 



B 



“ n ~ ^ ^ * ' 



CC, 



.T a x 



9 Tiissil- rax lull a 

roll rr a 

10 filisr p roe a 33 ccis Q 

p a r a :a a r ‘ i 
a ~ a i c * “ r .io acc. 
i: t arc so a a ruv?r 
tan i v iaah 

11 c c:t c :c 1 T-r cts: 
erotic :. o?>. r aai c - 7 - r 



a 



3 . j 



o .0 c 



3.5 =:C 



^ . 



23 7 



.23 :a./s;c 1 



7 i 



1/2 3 3 - 



a v 



k. W " - 



or. a 



0.003254 c, c 

1.25 



2 

T: a r C 



81 



Figures 4.4 to 4.12 represent the variations of th 

error covariances of each state variable. Figures 4.4 

through 4.7 show the three covariances for the relative 

positions, i.e., P^, , P 77 , P-- . Only the covariance 

of the two measurement case varies substantially. Thus 

one would expect that the gain components Gj ^ , G_^ 

would show only variations in the two measurement case as 

are shown in Figures 4.13 through 4.15. The trend noted in 

the set of graphs (4.4, 4.5, 4.5) and in the corresponding 

Kalman gains is also observed in the other covariance 

components. Thus the covariance components for the velocity 

(Figures 4.7, 4.S and 4.9) should larger variation is 

reflecting in the corresponding Kalman gains of 4-16, 4-17, 

4-1S. The covariances of the acceleration components show 

a similar development but there appears to be less variation 

in a^ and a_ acceleration covariances. In the case of 
ty tz 

a^, the variation of the Kalman gains are restricted to a 
much narrower range. The error covariance and the Kalman 
gain components in all three cases eventually converge to 
very small values. Figure 4.22 through Figure 4.30 show the 
results of the state estimations of the three estimators. 
Figures 4.22, 4.23 and 4.24 show the variations of the 
estimated relative position (R_, R , R„) . Figures 4.25, 

4.26 and 4.27, the variations of the estimated relative 
velocity (V , V ,1' ). Figures 4.28, 4.29 and 4.30 the 

' rx’ ry rz 
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variations of the estimated target acceleration (a , 
a , a t „) . The trends of the curves representing the 
estimated states are similar except for Figure 4.15 which 
shows a wide variation in the estimated V component of 
the two measurement case from the true state values. 
Generally, the estimated states of the estimator with 
two- and four-measurement are close to the true states, 
in spite of the wide variation of error covariance compo- 
nents of two -measurement case. The estimator with six- 
measurement vectors generates the estimated states which 
show almost same characteristics as the estimator with 
four-measurement vectors, but usually underestimated the 
states in comparison with the generated states of the 
estimator with four-measurement vectors. Figure 4.32 and 
Figure 4.54 show the curves of the control commands com- 
puted using the estimated states. The results of the six- 
measurement case have a wholly different form from the 
curves representing the computed control commands in the 
other cases. The normal acceleration commands computed 
using the estimted states with the s ix -measurement vectors 
initially have smaller values, but finally reach maximum 
value as shown in Figure 4.52. Also as shown in Figure 
4.54, the roll rate commands run from the minimum limit to 
the maximum limit. The large variations of the both control 
commands of case 3 during a short time interval result in 
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the worst miss distance, 15.4m. The control commancs in the 
other cases behave almost like characteristics representing 
that the control commands are increasing smoothly from the 
zero to its maximum control command, after two or three 
seconds then slowly decreasing to converging values. The 
relatively small variations of the both control commands 
during a short time interval comparing with the control 
commands of case 5 result in 9.7, miss distance in case 1 
and 8.7m, miss distance in case 2. From above results, the 
estimator with four- measurement vectors gives the best 
result, the estimator with six-measurement vectors the 
worst result and the estimator with two-measurement vectors 
almost same result as the of four-measurement case comparing 
with the result of the estimator with six-measurement vectors. 

In original measurement equation, for case 1, the 
missile has two-measurement vectors which give the informa- 
tion 'about the relative angles, 6^ and ^ , was assumed. 
These two-measurement vectors are non-linear functions of 
three relative distance components, , X ? , X_ . For case 2, 
the missile has four-measurement vectors which give the 
information about above two angles plus relative range and 
time rate change of relative range which are also non-linear 
functions of the components of six state vectors, X^ , X 0 , 

X_ , X,, X r and X, . For case 5, eventhough the missile 
has six-measurement sensors are assumed, the measured 
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vectors using the six-measurement sensors contain a 1 s „ 
state vectors which are exactly same kinds and n rm'.- er- 
the measurable state vectors using the four measurement 
sensors in case 2. As the angle rates, f n , y D , are 
included to the estimator as the measurement vectors in 
case 3, the both angle rates are non-linear functions of 
only six-state vectors, > X ? , X_ , X ; , X^ and X . 

The results of the estimator with four-measurement vectors 
and with six-measurement vectors show the almost same 
variations of the error covariance and the Kalman gain com- 
ponents of the six measurement case as of the four measure- 
ment case as shown in Figures 4.4 through 4.21. 

The Kalman filter in this work is a first order filter. 
Thus the components of the Jacobean matrix contain only the 
first order term, i.e., all higher order terms are neglected. 
As the order of the system is increased one would expect a 
more complex system to response differently. The above 
reasons affect to the underestimation of the states of six 
measurement case as shown in Figures 4.22 through 4.30. 

From the above considerations it can be concluded that as 
the increased number of measurement vectors is implemented 
to the extended Kalman filter algorithm, the result of the 
system may not be enhanced up to the expected degree due to 
the increased complexity of the system. The system with the 
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first order filter as a optimal estimator may contiiea 
to make less desirable results due to the effect of 
lectin® of the higher order terms in the filter al®or 
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PERFORMANCE EVALUATION Of THE CONTROL 
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This section will discuss the performance of the control 
system implemented in a bank-to-turn missile for each scen- 
ario. The maximum normal acceleration, the maximum roll 
rate and the time constant are the key parameters which 
reflect the missile capabilities, the noises to measured 
vectors are the parameters of the environmental effect, and 
auxiliary variables of some importance are then total engage- 
ment time and missile velocity. The mean miss distance 
determined from the 50 Monti Carlo runs was used as a 
performance standard for the estimators of two- (case 1) , 
four- (case 2) and six-measurement vectors (case 3) on each 
scenario. The simulation results are shown in Figures 5.1 
through 5.19. The sensitivities to variations of the para- 
meters will be discussed below. Since the simulation results 
show essentially the same characteristics for the tail -chase 
engagement (scenario 1) and for the head-on engagement 
(scenario 2). The performance on these two scenarios v r ill 
be discussed together for each parameter. The side- 
approach engagement (scenario 3) will be discussed sepa- 
rately . 

Figures 5.1 and 5.2 show the miss distance variation as 
a function of missile maximum normal acceleration. For 
scenario 1, case 1 shows a mean miss distance of 9 . Sm at 
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above log's, case 2.9m at above ISg's and case 5, 15. m 

above ISg's. Each case also shows that as the maximum iir.it 

of the normal acceleration decreases, the mean miss distance 

in case 1 is less sensitive than in the other two cases. 

For scenario 2, the result of case 1 shows the minimum mean 

miss distance is 10m and the result of case 2 is 17m at 

a = 21g’s respectively, the result of case 3 is 52m at 
max ^ 

a = 1 7 g 1 s . The trends of the curves show that the mean 
max ° 

miss distances in case 1 and case 2 vary in same manner, 
which rises sharply as the maximum acceleration decreases. 

On the other hand, the mean miss distances in case 5, vary 
smoothly above a = 15g's . One mav conclude from the 
analysis as follows: for scenario 1, the mean miss distance 
can be decreased since more information on the state vectors 
can be obtained from the increased measurement vectors in 
estimator for low maximum acceleration. In order to get the 
mean miss distance below 20m for all three cases, one needs 
a x °f over llg's. For scenario 2, as the measurement state 
vectors are increased in estimator, the mean miss distance 
does not decrease, possibly due to the effect of neglecting 
the higher order terms in the extended filter. As the mag- 
nitude of the values of the missile and target geometrv 
components becomes larger, the effect of the linearization 
becomes more pronounced. a max = over 19g’s is required to 
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to obtain the mean miss distance below 15m in case 1 arc 
case 2 . in case o , the mean mass distance does not 
below 10m. 

Figures 5.5 and 5.4 the variations of the mean miss 

distances as missile maximum roll rate varies. In scenario 

1, for case 1 and case 2, a mean miss distance of below 10m 

can be obtained when P is greater than 5 rad/sec. For 

max ° 

case 5, one has a mean miss distance of below 16m with 

P greater than 6 rad/sec. The trend of the mean miss 

distances is almost constant over certain range in values 

of P . The variation of the mean miss distance below 
max 

this range is a tenth of the magnitude comparing with the 
variation of the mean miss distances as a function of 
maximum acceleration. It is concluded that for scenario 1, 
the mean miss distance in case 1 and case 2 is not effected 
by the roll rate if the roll rate is above 5 rad/sec. In 
case 5, higher roll rate limit can reduce the mean miss 
distance. For scenario 2, the mean miss distances of case 
1 and case 2 are like the trends for scenario 1. However, 
the mean miss distance of case 5 has a minimum of 26m at 
P =4 rad/sec, then increases up to 36m then decreases 
slightly. This trend may be due to the effect of the roll 
rate lag, which is caused by the increased maximum roll 
rate and by the underestimations of the state variables in 
case 5. The other characteristics, i.e., the mean miss 
distance decreases with increased maximum roll rate limit 
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and for scenario 1, case 2 shows 1 ess near miss di f-.nc 
than case 2, hut for scenario 2, the opposite treni she . , 
can be explained with the same reasons as those of the first 
analsvi s . 

The variation of the mean miss distances with the time 
cosntant as a parameter is shown in Figures 5.5 and 5.6. 

For both scenarios, one has a flat plateau for low values of 
the time constant and a sharp rise with increasing calues of 
the time constant, i.e., for scenario 1, Figure 5.5 shows 
the curves of the mean miss distances slightly increase up 
to the time constant of 0.9 second, for scenario 2, the 
curves up to 0.5 second are plateau, then for both scenarios 
the curves rise sharply. For scenario 1, the mean miss 
distance of case 2 is less than that of case 1 up to 0.5 
second time lag, but above 0.5 second lag, the results show 
an opposite trend. Also over this time lag, the curves of 
case 3 for both scenarios are not sensitive to the variation 
of parameter up to the time constant of 0.7 second for 
scenario 1, 0.9 second for scenario 2. The general trend of 
the curves, i.e., as the time constant becomes longer, the 
miss distance becomes larger, is due to slow system response. 
From the figures showing the results, the time lag of less 
than 0.5 second is necessary to obtain the mean miss 
distance of the required order of magnitude. 

Figures 5.7 and 5.8 show the variations of the mean miss 
distances as a function of one sigma angle noise. For 
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scenario 1, as shown in Figure 5. - , the maximum m; 
distance is obtained for the six measurement case u i . ti. ■ 
miss distance being the smallest for the two measure ent 
case. Above about the angle error of 0.5 degree the miss 
distance is not a strong function of the angle error. For 
small error values, i.e., less than 0.2 degree, the curves 
seem to be approaching each other. For the scenario 2, the 
miss distance is a strong function of the angle error. For 
the two scenarios (Figure 5." and 5 . S) , the two measurement 
case has similar results up to an error of 0.4 degree. All 
cases show a strong rise in the miss distance with increas- 
ing error. The four measurement case seems pasticularly 
sensitive. 

The variation of the miss distances as a function of 
missile velocity are shown in Figures 5.9 and 5.10. During 
simulation the initial stand off distance was increased to 
keep the engagement time the same. One important aspect of 
Figure 5.9 is that represents an opposite trend of the 
vatiations in the mean miss distance to the general trend 
of the variation in the mean miss distance. Under the 
missile velocity of 700m/s, the mean miss distance is less 
than 10m for case 5, from 15m to 12.5m for case 2, from 15.7m 
to 14.5m for case 1. However, the curves for scenario 2 
show the usual trend, i.e., the mean miss distance of case 1 
is the smallest and that of case 3 is the largest among the 
mean miss distances of all three cases at a value of the 
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given parameter. For both scenarios, the general i : tud 
the curves slowly increases as the missile velocity- 
increases , except for one curve of case 1. It may he summa- 
rized the foregoing, considerations as follows: As the 

values of the missile and target geometry components become 
smaller, i.e., relatively less effect of neglecting the 
higher order terms in the first order filter and less 
contamination of the measured vectors to noises, the mean 
miss distance can be enhanced with the estimator of the six- 
measurement vectors. As the estimator algorithm becomes 
simple, the trend of the mean miss distances is less sensi- 
tive to the target geometry. 

Figures 5.11 and 5.12 represent the effect on the mean 
miss distance of the total engagement time. For simulation 
the initial stand off distance was increased to keep the 
constant missile velocity. The general behavior of the miss 
distances of all three cases slowly decrease as the engage- 
ment time increases. For case 1, the mean miss distance 
slowly decreases or is almost constant at below 10m. In the 
other two cases, there is more variation in the miss distance, 
but the general trend is still down’ward with increasing time 
to go. Note that as time to go increases for a given T, 
one has more characteristic time interval available reflected 

in T /t . The mean miss distance therefore should be less 
go 

as the total engagement time increases. This fact is veri- 
fied from both figures. 
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Figures 5.15 through 5. IS shoe the variation :Y v 
mean miss distances of a.ll three cases for side--;;; p roach 
engagement (scenario 3) as a function of each parameter . 

The large miss distances resulting from this, scenario 3 
motivated its separate consideration. General trends of the 
curves are similar to the other scenarios but with higher 
miss distance. This scenario is a strong test of the 
system. In this scenario, the control laws are subject to 
the error due to the limitation of the small angle assump- 
tion and the higher maximum acceleration capability is 
required due to the large variation of the PIC-miss in 
Y- and "-direction. Thus there are some unexpected trends. 
Figure 5.15 shows the variation of the mean miss 

distances for each case as the a increases. Although 

max ° 

the missile velocity is less than that of scenarios 1 and 2, 

the maximum allowable acceleration is increased. As shown 

in figure, the mean miss distance for case 1 is 11m, 

for case 2 is 10m, for case 3 is 15.5m only at a „ = 23g's. 

The figure shows the miss distance variations in this 

scenario are more sensitive to the a than in the ether 

max 

two scenarios. The mean miss distance of below 40m can be 
obtained at a = over 19g's, i.e., more a is 

required than in scenarios 1 and 2. This trend of the mean 
miss distances can be explained with the larger variation of 
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constant above P =4 rad/sec. That of case 3, however. 

max ’ 

continuous lv decreases up to P =6 rad/sec, then is 
almost constant. For scenario 3 one needs a maximum roil 
rate of about 5 or 6 rad/sec to have an effective BTT 
missile, which is higher than in scenarios 1 and 2. 

Figure 5.15 shows the variations of the mean miss distances 
as a function of time constant. Up to the time constant of 
0.7 second, the mean miss distance is less 20m. For all 
three cases, comparing with the results for scenarios 1 and 
2, the figure shows the curves of the mean miss distances 
are less sensitive in this scenario. The target accelera- 
tion vector lies in same direction as the missile velocity 
as shown in Figure 3.4. Because the components of the 
relative velocity are major components in computing the 
control commands, the more correct control commands may be 
computed due to the small uncertainty in and 

components. This results in smooth variation of the mean 
miss distance curve. The trend of the miss distances as 
the one sigma angle error increases, shows some different 
characteristics from the trend for scenarios 1 and 2 in 
Figure 5.16. The mean miss distances for all cases 
increase continuously as the noise input increases up to th 



139 



Miss Distance (m3 

150.0 00. 0 90.0 120.0 100.0 



iUL , ill I l\Oi wt 1 



^ ;^ c , o ? vir\|T naqr 

j.vwi iu-i ulN i w_ 



j 

H 

I 

1 

I 

4 



a 

a 



15.0 



■ p 



f 1/sec) 



Co C v : l . 



cr^ = S ( "/ sec 



CT 



C 



15 Idea) 



C ^ ^ O 
, O wl / 1 



o-.^an* . b ( aeg/ sec 3 . 28 > r cd/sec . 




20.0 
[ q's 3 



0 



FIGURE 5-13. 



MISSILE MAXIMUM ACCELERATION VS 

MEAN HISS DISTANCE 



140 



if) u > 



Hiss Distance (m) 

0.0 10.0 20.0 30.0 - 10.0 50.0 60.0 / 0.0 80.0 



SIDE-fiPPRCRCH 



ingp.gemen: 



X^Q.CSi 1/SEC) 0 - 3-3 (m) C7i"6:m/secj 




. ' 1 1 1 ' 1 ’ ! 1 I 

0.0 2.0 4.0 6.0 8.0 10.0 

P „ ( red/ sec ) 

max 



FIGURE 5-14. MISSILE MAXIMUM ROLL RATE VS 

MEAN MISS DISTANCE 



141 



Muss DLslance 1 rn ) 

0.0 100.0 200.0 300.0 400.0 500.0 



SIDE-APPROACH 



5AGEMENT 



Ej I ISl 



X“0 . Ob ( i/sec ) c-a'-btm) c^=5(m/sec) 

^a = or o = ’ 15(d8 9 ] G ,ax = 23( S' 3) 

.5 [ dac/ sec ) P K ,p X =6 . 28 ( red/ sec ) 

Enaccemert Tlme-S ( sec ) 

Missile Velocliu ~ SOD ( m/sec ] 

P 



1 

I 




FIGURE 5-15. MISSILE TIME CONSTANT VS 
MEAN HfSS DISTANCE 



142 



<m 



Hiss Distance (m) 

0.0 20.0 40.0 00.0 00.0 100.0 



SIDl-RPPRCRCH ENGAGEMENT Cf;cE 

X=0 .05 ( 1/sec ) a^=3(rn; cr^“6 ( m/ssc ] 

_ a .='j.= . 5 ( dea/sec 1 P MRy ”6 .28 ( rcd/sec ) 

S3 w i ihA 

t=0. BIsec) a . =23 {q's ) Q 

j Egacsment Time = 5 l sec) / 



Missile Velocity = SQQ(m/sec) ^ 




0.0 0.1 0.2 0.3 a.4 0.5 

Angle Ernorlo-^crJ [deg) 



"i — i — i 

o '. 5 



FIGURE 5-15. ONE SIGMA ANGLE ERROR VS 

MEAN MISS DISTANCE 



14 , 



Miss Distance ( rn ) 

0.0 200.0 400.0 600.0 000.0 1000.0 



SIDE-flPPRCPGH ENGAGEMENT CASE 



x~3 . 05 ( l/sec ) CT S “3im] 0-^=5 ( .-n/ssc 1 c=G.5 sec i 
a 9 ”<T “ » 5 ( deg/sec ) P M q X “5 . 23 t rad/ sec j 




FIGURE 5-17. MISSILE VELOCITY VS 

MEAN ,'irss DISTANCE 

144 



IILss Distance (m) 

0.0 200.0 - 500.0 600.0 800.0 1000.0 



SIDE-flPPROnCH ENGRGEi 



:nt CRSE 



X = 0 .Gb 1 1/sec ) o- R “3(m] cr^=S(m/sec) -t=*0.5lS3c] 




FIGURE 5-18. ENGAGEMENT TIME VS 
MEAN MISS DISTANCE 



145 



Miss Distance ( rn ) 

0.0 10.0 20.0 30.0 10.0 60.0 60.0 70.0 



x=0 . 05 i 1/ssc 1 a-o = 3 o^=6 ( m/sec ) 'C=Q.5.38cl 
o 8 = o 5 = .5 ( deg/ssc ) P ; ,. f j X =o . 23 ( rad/ sec 1 
o- = o-= . 15 tdea ) EncGcernenl Ilme=5(sec) 

9 2 w w O 




a. 3 



0.8 

Angle Rate Error (deg/sec) 



r 



1.3 



FIGURE 5-19. ONE SIGMA ANGLE RATE ERROR VS 

MEAN HISS DISTANCE 



146 



one sigma error of about 0.4 degree. \bove the one sigma 
error of 0.48 degree , case 1 decreases then rises sharp!/, 
case 2 continues decreasing slowly, case 5 increases 
continuously. The variations of the mean miss distances in 
scenario 1 are almost plateau and in scenario 2 are almost 
constant up to the one sigma angle error of about 0.4 degree 
then rises sharply as shown in Figures 5.7 and 5.S. This 
reason can be deduced from the fact that as the one sigma 
angle error becomes large, the same magnitude of the two 
state vectors in different direction, i.e., relative 
distance in X-direction and relative in Y-direction, having 
the same order of magnitude may be contaminated to the noise, 
on the other hand, the other scenarios, only the components 
in the X-directicn being relatively large are contaminated 
to the noise. From this fact, it can be concluded that the 
missile in a side-engagement case is more sensitive to the 
angle error than the other scenario. 

Figure 5.17 shows the variations of the mean miss 
distances as the missile velocity increases as a parameter 
(i.e., holding the total engagement time of 5 seconds and 
changing the relative position of the target). Up to the 
missile velocity of 700m/s the curves of the mean miss 
distances are plateau, after that velocity the curves rise 
sharply. Figure 5.8 shows the variations of the mean miss 
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distances as a function of the total engagement t ; : n . , 

holding the constant velocities of the missile and 
target and changing the relative position of the target in 
scena ri o) . 

Figure 5.13 shows the mean miss distances rise sharply 
over the engagement time of 6 seconds. This is somewhat 
unexpected and may be an artifact of the implementation 
procedure. In other scenarios the mean miss distance as a 
function of the missile velocity increases slightly as the 
missile velocity increases and that as a function of the 
total engagement time decreases slowly with increased 
engagement time. From above analysis one has a conclusion 
that the computed control commands should converge in the 
final time as the computed commands of the two- four- 
measurement case shown in Figure 5.52 and Figure 5.54 to 
obtain the low mean miss distance. However, it can be 
expected in this scenario that the computed control commands 
do not converge possibly due to the violation of the small 
angle assumption in deriving the equations of the control 
commands (I.e., in this scenario the „ component is 
large in Equation (3.7), that causes the large mean miss 
distances). However, in the case of relatively small magni- 
tude of the missile and target geometry components, the 
error caused by the violation of small angle assumption is 
relatively small comparing with the errors caused by the 
contaminated of the measured vectors to noises, 



limitations of the control inputs, and the estimatia 
the state vectors using first-order filter. This fact 
verified with the results that the mean miss distance is 
11m for case 1, 10m for case 2, 14m for case 3 at the 
missile velocity of 600m/s and the engagement time of 5 
seconds, as shown in Figures 5.17 and 5.18. 

The final Figure 5.19 shows the variations of the mean 
miss distances for case 5 of all three scenarios as a func- 
tion of the one sigma angle rate error. The result repre- 
sents that the variations of the mean miss distances due to 
increased input values of one sigma angle rate error are 
almost constant for scenarios 1 and 2. For scenario 3, the 
mean miss distance varies at constant rate of inclination. 
This slope of the curve is relatively small comparing with 
the slopes of the mean miss distance curves as the other 
parameters change. From this analysis, it may be concluded 
that the exposure of the measured vectors to the angle rate 
error noise is neglegible in the case of present study. 
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CONCLUSIONS 



VI . 

A simple but effective biased guidance lav/ and ttrze 
extended Kalman filter equations have been derived from the 
optimal control theory for a bank-to-turn missile with zero- 
lag and with 0.5 second time lag autopilots for pitch accel- 
eration and roll state. 

The equations of motion are linearized around the present 
orientation of the missile with the assumption of small 
future roll angles. During computer simulation, the optimal 
control laws and estimators were tested separately invoking 
the separation theorem. In the simulation of the system 
three optimal estimators were used, i.e., estimator 
with two measurement vectors, with four-measurement vectors 
and with six-measurement vectors. The system has been eval- 
uated for three scenarios, tail chase engagement, head on 
engagement and side approach engagement. 

From the analyses of the simulation results in Chapters 
5, 4 and 5, it can be concluded as follows: The control law 

was successfully simulated for a hypothetical bank-to-turn 
missile with pitch acceleration and roll rate autopilot 
within the imposed limits on three scenarios. Miss distances 
with zero-lag were negligible for all three scenarios 
(below 0.5m). For the time lag case one had miss distance 
of 5.9m for scenario 1, 0.8m for scenario 2 and 32.9m, with 
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0.5 second lag and with a x = -Og's. By imposing 
higher maximum g of 25 on the scenario 5, the miss 
distance was reduced to below 5m miss distance. One would 
expect a high g requirement for a side engagement situa- 
tion as in the case of scenario 5. It is evident that the 
missile's tracking ability against the target is very 
sensitive to the missile and target geometry, the missile's 
manuevering capability is also a very important factor. 
Optimal control laws applied to the side approach engage- 
ment place a severe strain on the system from the viewpoint 
of the small angle approximation. Increasing the number of 
measurements did not result in increased performance in the 
sense of smaller miss distances for the missile except in 
isolated cases. This may be due to the increased com- 
plexity of the system as the number of measurements was 
augmented. Further, in this study only first order extended 
filters were implemented. It is possible that better 
performance could be obtained at the expense of increased 
computational load, by using a second order extended Kalman 
filter. 
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APPENDIX A: PROGRAM LISTING 



This appendix provides listing of the computer program 
used in the present study. Only one program for the six 
measurement case is provided. Except for a few differences 
in the subroutine filter the programs for the two other 
cases are identical to the program included here. 
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//? 
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* / US , ,E1 *H2 , Z3 , i : , r 5 ,E 6 f E7 , Eo , Z 9 , 2 1 , P2 , P3 , 24 , ?:> , ro , ?7 , ' - 

X (9) ,? 
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C 

C ** 
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INPUT DATA 

DT I = . 0 5 
NC AS E= 1 
N K A X = 1 1 C 
KF PIN? = 1 
L r F I N T = 5 
1 1 LCT= 1 
TFT 0-0 - 
PS!0=0 . 
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XNC-0. 

YKG=0. 

ZKOO. 

XT 0 = 2500 . 

Y I C=0 • 

ZT 0 = - 1 00 . 
YIXC=5CC. 

V T Y 0 = 0 . 

VIZ 0=0 - 
E 1=. 003254 
B 2= 1 , 2 5 
A T X 0 - 0 . 

AI Y 0=4. *9.8 
A 120=0 . 
CTi=.G5 
Cl Y = . 0 5 
CI2 = . 05 
SAT =5. *9,8 
CT-1-/20- 
E21=.0G2b 
RZ2 = , OC81 
E Z 3 = . 0026 
RZ4=,GC87 
K 2 5 = 3 . 

R Z 6 = 6 . 

SE 1 = . 0 0 2 6 
SE2=.0C37 
Sr3=.0G26 
SR4=.0C87 
SE5=3. 
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0-0 



AKIM=- 3. >9.3 
i .'1 A X - 5 . 3 c 
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£ * * ********* 1 :*** 
C 

C INITIALIZE 



;*;,**:* * 4 * * * *** ■»»:******** i<** * -* ****** 



A , i 



'AT BICES 



100 
1 1C 



120 
1 3 C 



z c no 1=1,9 
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= ? 1 


A |6, 9 


-12 


A <9/9) 





S!s:«:Sil 

Q (3,9) =Q 13 
C (6,3) = £12 
Q ( 6 ,b) =Q 22 
£ |o, j) = £23 
Q (5,3) = £13 



3 , v 3 3 



1S4 



13 4 

5 

6 
7 

B 



0 { S , b) =Q 2 2 
C (9,5) =-33 

WHITS (6,5) 

*RII2 (6,6) 

DC 13 + i= 1 ,J 

-axis (b, 1) 3(1,1) ,g |I,.2) ,c (i ,3) (i, *) ,■* ii,5) , * *i, 

*,2 (1, 3) ,C (I,;) 

CO MIN r JS 
FCHSAT (MM 

FC3;m (//55X, 'CHIC-INP.L 3 AATRIX'//) 

FORMS (3? M , c) 

FCrMI (//4,X, M MT7.IX *113 PSrUDO N C IS3 , <?3'J=M 

ADD PS 5 U L C NOISE IC I 3 AT SIX 

'25 



F 1 1= (JF3U**2) * III' 
3 12 = U?Si: + *2) * (37' 
322= ussa«*2) * c: 



**->/?• 
/2 . 

--2) 



c (1,1) = G I3r 1) *511 
W (4,2 =Q U , 2 + - 1 ] 
2 U,3) MM 1 



V - 

v i J , 3 



C (4,3) = 3 (4,3) > r 2 2 
U | 4 , O ) = U <4,4} 

Q (C, o) =^ (6,0) +522 

Q (1,3) =Q ll, i) + ? V 2 - 

C (2,4) = v U , 4 ) + rK 
3 ( J, o) =>2 (3,o) 

Q <4,11 (4, 1) - M 2 

C I - , 2) =C |5,.M tK 

w (t , 3) =Q <c , 3) + F 1 ^ 



WRITEJ6.S) ^ " 
£ C 134 1 = 1,5 



? £ C 



WRITS (6, 7) 2(1,1) |I,2J , C (I ,3) ,v. (I, 4) , ; (1,5) , g (I, 

l ,C 11,3) ,C (Im) 



135 CCNTINUr 



147 
1 5 C 



DC 150 1=1,9 
DC 140 3=1,9 
AT ( T , -j ) — A | <j , l ) 
CONTINUE 
CCNTIN US 

5 3 ITS (6,5) 

START CASS LOOP 

DO 950 111=1 , NCA5E 

INITIAL IIS 

H = C 

K= ?F3I NT - 1 
L=L?RINT-1 
ISTC?=0 
DT=DIT 



I?(NCASS.SQ. 1) 
KFSINT = 1 COuO 0 
LF5INTM CCGOG 
I F LCT=0 
K = C 
1 = 0 

16C CONTINUE 

I (1) =0. 

Y (2) =T HTC 

Y (3 =ps; c 

Y |4)=7s0 
i (5) =xuo 

Y (6) =YN0 



GO TO 160 



~ ) , *£ ( 2 , M 



25.0//) 



o) ,2 (1,7) 



155 



c 



c 



c 

c 

c 



c 



1 7 C 
180 



C 



c 

c 

c 



190 

2CC 



7 0 C 



C 

c 

c 



c 

c 

c 



Y (7) = 1X0 

Y <cf = 7 T X C 

Y ( 9 ) = 7 : Y 0 

Y (101 = 7 120 

Y (11) = X 7 0 

Y ( 12) = Y I C 

Y (13) = ZTC 

Y (14) = ?H I C 

Y (151 = 0, 

Y (16) = C * 

CHAW RANCCI 






CALL L NC?X (I £ EEL , V 1 , 1 
C i. w •' * - (i-— i z z L f c f 
CALL L N C r Y (IS SIC ,73,: 
CALL LMC 5/. ( 1 3 r r C , 7 a , ; 
CALL L A* C c £1 ( _ 2 .. i , <* 5 r : 
CALL L N C r E ( 1 3 i l L , 2 6 , .* 

INITIALIZE ? 2 AT SIX 

CC 180 1=1,9 
C C 1 7 J 3=1,9 
r <I,J) =0. 

CC N L IN L I 
ccmi:;ui 

P (1, 1) = 3 c CS * ~ 2 
? ( 2 , 2 ) = S£C 2— 2 
f (3,3) =S IC3**2 
2 (4,4) = 5 7 I L * * 2 
? (3,5) =S7IL**2 
I j o , o i =S 7 EL* * z 
? (7,7) =SACC**2 
£ (o,3) — 3 A C v, * ^ 

? (2, 3) =SACC**2 



-i A , i , 1 ) 

a x , i , n 

AX, i, M 

a :c , i,i) 

AX, 1, 1 



2 CO 
190 



j - i , - 



DC 

GAIN (I » J ) = 0, 

CCMIN3 c 
CCNIIM US 

START TFAOICTCFT ICC 2 

CCN IIN US 
S = N + 1 
K=K + 1 
L=L + 1 



CEBIN2 I M 

T= Y (1) 

T R 1 = Y ( 2 ) 

I S I = Y l 3) 

VK = Y (ii J 
X 2 = Y (5 
Y^ = Y (b 

7 T Y = Y <>) 
VTZ=Y 1 0) 

:<t=y (ii) 

YT = Y (1 2) 

ZT= Y ( 13 ) 

PRI=Y 114 ) 
I 2 =Y ( 15 ) 



■GHATIC I V API A 5LSS 



INERTIAL TO 2CDY 2X1 



TR A NSFC STATION 



A 1 1 =DC O £ (THT) *CCC5 (PEI) 

A 12= DC OS (THT) *LS1N (221) 

A 13=- j SIN IlH I ) 

A21=DSIii (PHI) *C SIN (TFT) *DCOS (LSI) -DCCS (£ 



* DSIN T (?3i) 
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A 22 =03 1 N i PHI ) 
A 2 3 = 05 I N (PHI) 
A 3 1 =CC 0 S (Piil) 
A 3 2 = OC 05 (PHI, 
A 3 3 = CC 03 (PHI) 


* C £1 
*CCC 
*0 £1 

* £ C ■ r 

* ICC 


v “ x = a 1 1 * v y. 

V H Y=A 1 2 * 7 H 
V 2Z= A 1 3* VI? 




CC. V FJT 2 TFUZ 


CC 7P 


?x=xt-x:i 

3 7 = 71- i ;•! 
EZ=ZT-Z3 




7SX= 7T X - V X X 
7RY=VT 7-^7 
7F2=VTZ-V^Z 





) * E 0 I'I’.iT) * D 31 N (£51) ^DCCS i SHI) 



03 (351) 



-* DCC3 ( P£I) + DS I { 
* CSIN (251) -53 IN ( 



L 5 75CIv 



EHI) * 
2 d I ) * 



Co 13 

coos 



( ? b il 

(?5I; 



ATX=ATXQ*tZX? (-lT? 4 T5 
A13=AI 7 0*03 i? (-CIi*!) 

ATZ=A7Z0*CLXP ( -CCZ -T ) 

X2 ( 1) = SX 
X 2 ( 2) = tt 7 
X2 (3) = EZ 
X 2 (4) - VHK 
X 2 i5) = 7E7 
X 2 ( 6 1 = 7E 2 
X 2 (7) = AT X 
X 2 (8) = AT 7 
X2(^) = AT2 

2 GO- ( rX*7?.X + r 7 * 7 3 Y * ?Z*7 3 Z) / ( 7 r X**2 + 7? 2 : 

; FCSH X EASCN3H EXT 7ECIC? 

S 1=03331 (SX^E+SY 51 *!) 

S = DS,)3 I (£5**2 *E 7 -42+SZ**2) 

5 ET = (3 X * 7 5 X + S 7 * 7 E 7 + S Z * 7 S Z ) / S 
THTS=-DA! AN 2 (SZ,S ') 

PbIS=D ATAN2 (S i ,3 >) 

T3TSD= ( (-ESIN (IRIS) * lCOS (FS IS ) *SX*VR X) - 
*5Y)+ (DCO£ (THIS) *S2*VSZ) ) / (3 ^ S ) 

££ISL= { ( - ESIN ( 2 2 IS) * 21 * 7RX) + ( LCC3 (PS IS) ; 
EEIS=S 

3DIS0= (SX*VRX4£ ) * 7 fv Y +SZ* V 3Z) /S 

Z (1) = TH 1 £ *D3LE (VI ( N ) ) * S Z 1 
Z (2)=:HTSE + EE1£ (V2 (N))-SZ2 
Z ( 3) =? 51 £ +D3I5 ( V2 (*.i) ) *S23 
Z (4) =?Si£E+CcLE ( V / ( N ) ) * 5 Z ^ 

Z (5)=aCIS * D b L I <*I (N)) *SZ5 
Z ( c ) =3 DIEC+D3LE ( V 2 ( N ) ) * S Z 6 

: INITIALIZE STATE \ E C TOE 

[ IF (N-G I. 1) GO TC 750 

CALL LNCcX (I5ZEC £ > V , 6,1 .1 ) 

X ( 1) =3X+E ELZ { 4 V ( 1) ) *5203 
X (2) =3 7* CELS (VV (2)) *SrOS 
X (3) = RZ + CELL ( T fl * (3 ) * £20 3 
X ( 4 ) = 7 RX+CSLZ (** (in ) ?S7 EL 
X (5) =7R7 + C3LZ ( »* (5) ) *S7S L 
X (6) = V EZ ♦ £ LL Z ( V 4 (6) ) -‘37 I L 
X (7) =0 . 

X (81=0. 

X (S) =0. 

750 CCSTIifUE 



: + 7RZ *-*2) 



(3SIN 

‘37*7 



:I (THi 



f) ) 



S) *LSI 
/ {5*S* 



II 1=DCCS (THIS) *CCCS 
£ 12=DC0£ (THIS) * D 5 1 N i 
E1i=-DSIN (THIS) 

XLCS = D 11*X (1) + E12*X < 2) *D1 3*X ( 3) 



ItSIS 



N ( ? 3 1 S ) * S 7 3 7 
CCOS (ThTS) ) 
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GZ'lZii! 
CALL : 



uic: 



; u i a n c - cc t.iAiiDs 

. (X, :GC2 , AC , f C , C T) 



I F { AC , G T - * A A 2 ) A C - A H AX 
If {AC. LT.-AHIS) AC = -.ulIS 
I F < PC - G T . + P :! A X ) = C- z >. A X 

If { PC. LT . - :! AX) :l = -E£A<< 

If (TSPI-N.LE. 0.) 2. V =PC 
If (TACC. LE.O.) AX = AC 

ST03E VAHIA31IS ?C2 f LO f T ' 

c ^ I * c 7 . 3 

TT (U) =T 
AAC (in = A c 

esc h) = p c 

A A X (N) = A ; J 

es; # <o — 



5 A A K t If 
TGI (II) 
TG2 (If) 

ui = 

Uz no = 
L’3 (A) - 
L*4 (N) = 
C5 ( X) = 
36 (II) = 
77 (Ii) = 
G6 (II) = 
U9 |.\‘) = 

El cn = 

5 2 (>() = 

eg cn = 

E4 cn = 
E5 |N) = 
E c ( ii ) = 
Z 1 {If) = 

E 6 mi 
2S i:r 
f 11= 



= fc h I D 
^=T Q C 
= TGC2 
3 X 
2Y 
^ z 
73 .X 
7 2 7 
V K 2 
AT X 
AT i 

lill 

m 

X ( ;) 

M ( c ) 



X 

) =-< . 

05 C3T (? ( 1 



P22=D3.;ai 


(? 1 


(2 


P 33 = OS C 31 


(P 


(3 


? 4 4 = 0 5 T 2 T 


(? 


(4 


P 5 5= 03 :ai 


(? 


(5 


P6c=QSC31 (? 


( o 


P77=D3C3T 


(? 


(7 


? 6 3=Di CR T 


{? 


(5 


299=35 C2T 


(P (9 



, D ) 

nil 

nil 

nil 



1 1 i 



Pi (S) = 

£2 |M) = fC^ 
P3 tin = P3 3 
2 4 (>,’) = ?4 4 

1111:1 = 



?7 cn = 
£8 (0 = 
?S IN') - 
Gil (in 
G21 Ii) 
G3 1 (Ii) 

G 4 1 (M) 

G 5 1 X) 



? 6 c 
27 7 
? 3 8 
P 9 9 

= G A III ( 1 ; 
-G AIN ( 2 j 
= G A I II ( J , 
— G AIN (4 , 
= 6AIN ( 5 d 



LISE 

If (X. 
K = C 
TBT D~ 
? EID ; 
PHIC-- 
« 3 1 IT 
« BITE 
jiSITB 



BY 111 

LT - Xf 5 

? HT * 5 7 
=? 81*57 
? H I * 5 7 

(6,20) 



P r ' 



I NT) 

. 3 

j 

' J 

1 , C A 

T , X 



GC TC 250 



Ii CASE = 1) 



:<ICS / pC,TGq2 / AC,AX < PC, Eg.rdID 
/ i* i." Z , /3 a, /ai, / : « I ( f Aio 

1) (2) ,Iv(J) ,31-4) ,.<(5) ,3 (d) , a ( 7) , 



A (b) , X (j) 
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10 

i 0 

2.5C 



3 0 C 

1 Q 

•40 
3b 0 



> 3 3 , ? 4 -i ,±55, 566,177, i J 5 , J * 3 



UOC 



c 

c 

c 



50 



c 

c 



• 3112(6,20 x , E 1 1 ,1 
? C F M A T (/ 1 C71 3 . 2) 

F C s M AT (10113.2) 

CCNTTN UZ 

IFJL.LT.IFSIND GC TC 350 
SEITZ (6 , 2 C) : 

DC 3 U 0 1= 1 , ) 

• SITS (c, 4C) J A IN (1 , 1 ) ,3 A I X {I r 1) ,3 a! 3 (1,1) , 3 Alii \ Z , 1; # 3\ :n ( I , 1) 

♦ gaijiji, J) 

FC3/.AT (/^57X t 1 GAI> X A TR I . 

F C 5 M AT (52X,oc - 2.4) 

C C N T I N U_ 

CHECK FOB TZ 5 MI N A1IC N 



(T=« , J5 . 3 , •) •/) 



IF (SDT.GE,: 

IF (iovi.oI, , 

L T = IGQ 

is:c?= i 

C C N II '.1 US 



.C.Ca.IGC.LE.O.O) GC TC 300 
1) CC XC ^uO 



90 0 



I N IEGR AT E rEAJECICftl ONE STEF ?HEAD 

CALL I N T Z G (Y,c) 

I? (I3TGP .GT • 0 ) 3 C TC 930 

I r ( N . G Z . N ? A X ) GC TC 500 

EXECUTE KALMAN* FI1TEF (UPDATE ETA IE 

Coil r 1 L 1 £ > ( X ; C f C ; o ^ a x ii ) 

GC TO 70 C 

IE EMIN AL £51 NT AND PLOT 
CCNTI'.TU S 

iSI|R.,.«o, 

D Y M ( 1 2) — Y 



V EC TC R ) 



CZ = Y (1 3) -Y (7) 

DK = DS*!RT iEX**2+Ci**2+DZ**2) 
X MSS (III) =0X 
YKI33 ( 1 1 1 ) = D Y 
ZMI3S (III) =02 
CMIS3 (III) = D M 
• SHE ( 6, 5C) I . TX , D Y M2, 0 Z 
FCSM AT (// 5X , f T= 1 , X 10. J , 5 
M 1 D Z= 1 ,?1C.3,5i f MISS=' ,7 

I? (IPL0T.S3- 1) GC TO 950 

CALL PLOTG (TT ,E 1 , N , 1 ,1 , 1 , 

* t 1 1 , 0 . f 0. ,0. , 0 . ,b. ,5.) A 
CALL PLC1G i IT , L 1 , N , f , 1 , 0 , 

‘ / 1 1 r 0 • , Li - , 0 . , L . ^3 . ) 

■ : PLCIGCTT^Zz^,! ,1, 1, 



CALI 

* , i _v 



'iCTG(Tr,2x 

CALL ?lGTG jff »2* f 1, 0, 

-*,11,0, , u . , b . , 5 . ) 

CALL PLOTG (TT .23 ,N , 1 ,1 , 1 , 

* ■ ' n \ ■> 1 tz\ 

^ , I l g 'J • I V < / ' o ( U « J J . ( j . j 



CALL PLOTG (TT , Zz x 'A,2 , 

^ , 11,0# ,c*,U.,0.,b.,b.) 

PLOTG (TI ,E4 :; f 1,1,1, 
3. , C , , u. , C . , b . , 5 . ) 
PLCTG (TT ,04 £ N , 2 , 1, 0, 

A * r> £ c v 



CALL 

♦ ,16,0.,0.,J. , C . , 5 . , 5 . ) 

CALL PLOTG (TT , Eb t N , J , 1 , 1 , 

MiiMcId (f? ^£3 < o . 

*,1b,0. , 0 . , J . , 0 . , b . , 5 . ) 



>:< = 


' ,?10 . 


3 , 5 X 


, 1 0 Y = ' , ? 1 3 . 3 , 5 X 


10.3,5 


;< , * c a s 


£=\ 


17) 


f 71 y n 


(SEC) * 


, 10, 


' RX (METERS)' 


I'T'l -1 C 


(SEC) ' 


,10, 


MIX (METERS) » 


f T I M E 


(SEC) ' 


, 1C , 


1 RY (METERS) ' 


'TIME 


(SEC) ' 


,10, 


' RY (METERS) ' 


'TIME 


(SEC) ' 


,10, 


' HZ (METERS) ' 


'TIME 


(SEC) « 


,10, 


1 RZ (M ST SRS) 1 


i T T M E 


(SEC) ' 


, 1C, 


' VRX (METERS/SEC) 


'TIME 


(SEC) » 


,10, 


» VRX (M2T2 HS/SEC) 


'TIME 


(SEC) » 


, 1C, 


' TRY (METERS/SEC) 


"TIME 


(SEC) ' 


, 10, 


' VSY (METERS/SEC) 


IT^Mt 


(SEC) 1 


,10, 


’ VSZ (M 5TERS/SEC) 



non 



i - #3 « 



CALL ?LCIG(rT,Jo,A,;,l, 

*call ?{.ctgt :< i T, 

*,13,0. , J . , .) . , G - , b . , 5 . 1 


0 / ’TI 


ME (SIC) 


* , 7 C , * 


VRZ {.IE MS/ 


oil) ’ 


1 , T I 


M (SIC) 


’,10,* 


ATX (MELEES/ 


GES*-M 


CALL PLC1G (XT , C / , N,2 ,1 , 

♦ , 19,0. ,0. ,0. , o . ,5. ,5.) 


J , ’TI 


ME (SEC) 


T 1 0 , • 


ATX ( M ETEES/SEC **2.) 


CALL PLOTS (TT , I c ,1,1 ,1, 
*,19,0. ,0. ,J. .^.,5.) 

CALL PLOTG (XT , Lc , !J . x , 1, 

* , 1 b . 0 . , u - , v, . , G . , b . , b . ) 


1 f ^ ^ 

1 / A - 


EE (SEC) 


M0, ’ 


ATM (XETEHS/ 


SEC- A P) 


0 , TI 


ME (SEC) 


TIC,’ 


ATX (MEIERS/ 


3EC^^2) 


CJL- 2 L C 1 C- (r; , hi , ,1,1, 

cal! pLctGjt VmM , 1 , 

* , 19,0. , G . , j. , 0 . , i . ,b .) 


1 , TI 


ME (SIC) 


TIC,’ 


ATS *11X155/ 


S EC * ^ 2 ) 


j i ^ r 


MS (SEC) 


T 1 0 , ’ 


ATE (MEISRS/S 2C**2) 


CALL ? LC TG tT * / P ' j A , 1 ,1 , 

* ,0. ,0. ,0 . ,J. , i . J.) 


1 1 <T> T 

if L. 


ME (SIC) 


T 10, 


’SORT (PI 1) ’ , 


f 


CALL PLCTG iTI,r2 , A , 1 , 1 , 


1 , TI 


ME (SEC) 


TIC, 


’ SQET { P I 2 ) ’ , 


* 


CALL PLCTG (TT, 1,1, 

* , C - , 0 • ,0* , u • , b . ,~ .) 


1 , TI 


ME (SEC) 


TIC, 


’ 5 G ?. T (P 33) ’ , 


3 


CALL ?1CIG (TT £ T 4 , R , 1,1, 
^ u , b • .) 


1 1 T 1 T 


ME (SEC) 


TIC, 


1 50ET (P4J) ’ , 


9 


CAL^ PLCTG iT I £ Id £ , 1 , 1 , 
* , 0 • , u . , j • , u . , b . , b . } 

CALL PLOTG (TT , To , A , 1 / , 
*,0. ,J. ,J. ,J., < 3.) , 

C A L i. PLu i LI , t 7 . ,i , I , i , 

* / c . ,o. ,0. 

CALL PLOTG ( T T , T o , G , 1 , 1 , 

, b . , 5 . ) 


1 , TI 


ME (SEC) 


T 10, 


•s ;RT ( P 5 5 ) ’ , 


) 


1 , ’TI 


ME (SEC) 


T 10, 


’5/RT (?5S) ’ , 


9 


i , t: 


ME (SEC) 


TT, 


’S ORT (971) * , 


9 


1 , TI 


ME (SEC) 


TIC, 


1 S v «' i ( - S 3 ) ’, 




CALL PLCTG (TI , E 9 ,>,1,1, 

*,c-, 0 . , 

call plctg (i : , s ;c , % i 


1 IT T 

<f . J. 

, 1 , * T 


M: (SEC) 

I ME (SEC 


TIC, 
1 T 10 


’ S Q R L ( P 9 5 ) ’ , 


o 


*,’:i 03ML ACCELII A HC'i ( 
CALL i'LCTG(TT,£:f. , 1. , z , 1 


.1/5EC 

,G,’T 


* 4 I) ’ ,^0 
tie (s_c 




, 0 . , 0 . , 5 . , 5 . 


) 


-,’NOSaAL ACCELE/.ATIC:, { 
CALL ?LOTG(TT,rrC # E, : . 1 
* , ’ r CLL SATE ( ? AL/SEC) 1 , 
CALL PLCTG (TT , EP 5 , A , S , 1 


M /SEC 

] I 


* 4 ^) ’ , 3 J 
IT jSEC 
, C „ , j . , j 

I2E (SEC 


) T 10* 


, 0 * / o . ,5. ,5. 


) 


\ 9 " o ; 

f ^ * - 


* r b . . 5 

) T 10 


-) 




*, ’ E CLL RATE (SAT/SEC) ’, 
CALL PLOTG (T r , 5 A G B ,5. , 1, 
* , ’ EA:(K a AGLS (LEG) ’ , 16 f 
CALL PLCTG (IT ,TGL ,11 , 1 , 1 
*, ’TIME TC GO i SEC) ’ , 1o, 
CALL PLOTG (TT , I G 1 , N , L , 1 


19,0. 

3:!5! 

5.’:o: 

} i ^ 

. J r * 


, C . , 0 . , G . , b . , 5 
TIME (SEC) 1 , 10 
, C . , 0 . , 5 . , 5 . ) 
ICE (SEC) ’ c 10 
,G. ,0. ,5. b.) 
IME j SEC ) « t 10 


•) 





CALL ? LC 1G (II ,G1 1 ,N 

* , 1 C- (2, 2) *,b, 0. ,0 . , G 
CALL ? L 0 1 G {Cl , G2 1 /i 

* , * 0 (5 , 2) *, b , C . , C . , G 
CALL PLCTG (T? , G3 1 , l\ 

*,'G (6, 2) ’ ,o z G . , C . ,C 
CALL PLOTG (L T , C4 1 ,N 
* , *S (3, 1) M, C. ,C. ,0 
CALL PLCTC- (TT , G5 1 , G 

t ALL PLOT {0. , 0 . , 9§9 



1 1 1 i r-> r \ Z 

,U. , 5 . , D. ) 

1,1/1 f ' T I i* £ 

,0. , 5 . ,5 . ) 

M, 1 , ' T I :i 5 (SEC) 

ryr f |vL 

, G. , 5 . , 5 - ) 

1 1 1 I “* T V "5 

1 r 1 ' c ' - 1 v *- 

/ v , 5., D. ) 



(SEC) 

(SEC) 



(SEC) 

(SEC) 



3 .) 

z 10 
, 10 
, 10 
, 10 
, 10 



95 C CCNTIGUS 

COMPUTE KISS CISIAKCZ STATISTICS 

17 (NCAS2-EQ. 1) GC TC 975 
CALL SCAT (XMISS, EC AS : , X 3 A r , X 2 IS) 
CALL 5 T A 1 (Y(il££ , UA3E, Y3AE, :5 AS) 
CALL SCAT (I MISS, GCAS r,S_ A?, 2 ? M i) 
CALL S I A I (0(1155 , oCASE , DEAR, DBMS) 
* SxTE ( b , b) 

Vi 5 I IE (6, 

A SITE { b , 



sc] 


X 5 A S 


TC) 


YEAR 


sci 


Z 5 A R 


9Ci 


DEAR 


7 15 


X , T y I 


SI, 


’ x-cc 



WRITE ( 6 , SCI QEAITIB.MS 

55 E C R-.M AT (/ / 1 3 X , * (T 5 5 C 1ST A Vi CE STATISTICS’ 

6 C FOSSA I (/5X, ’ X-CCMBCSENT ' ,5X, ' ME A 



/) 

, r c . / , b X , ' E M S = 1 , F 3 . 2 ) 
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mf/i n 



7C ?ca:i\r (/5 a, • i-cc:^c..z:,i * , 5x 
cj ?c?;nr t / Z ' n , ’ z-cc^scnznt* ,5x 

30 ? C S 2 A T (/ 5 X , ’ C C 1 A L MlSa 1 ,3X 

c 

CVTINUS 
1C? 

NC 



iAii^' 
cAN=* 
SAii = ' 



* - - f • ' 

5.C 



;> MS = ' 
M S * ’ 
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:> u 
IOC 



S&HROailSr GZXZ 2 ( i , 7 Gvj , AC , 7 C ,C 7 ) 

12; lie IT 5£AL*£ t 3 - £ , C- 2 ) 

DIMENSION a (9) 

CC^GN/T hFSi/ A; 1 ,A2:,.\23,kl 1 ,*22, A3 2,31, 2< 

G = 9 . 8 
eU = X (1 ) 

S Y” X (2 / 

R2 = X (J) 
v s7= v / : . 




f 1 i-J.^TGC/ ( J . ^ c 1 * I 3 C * - 3 ) 

AC=-r 1 *2 £2 

DrnI=DATAN^ ( 25 Y, - 7 r 2 ) 
F2=7.*AC ! MC*TGC*U50**3*3.*&1 
r 3= (AC * A C *7 GO * * 5) ♦ ( c 3 . * 3 2 ) 

?C= ?2 * DPHI/r 3 
GC 70 IOC 
rr\TT ; Mr 



Vj A. . 'J I J 

C C N i I :i 'JE 
AC= ) . 

? C = 0 „ 

CC MINUS 
3 FTU 2 :. 

£ 2 i C 
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55 

50 

65 

7 C 
75 



Z1 (15) ,Pi2 (1o) ,5 )) 



SUH3C3TIX2 I N T 2 C { Z E I : , 3) 

IMPLICIT HEAD’? ,C-C) 

chins ion dein ( to) , ceout ( 16) r . v . 

COMMON /I VC/ AC, PC , A* 30 , AT 'id , .VI ZO ,CTX 
G = ': , 3 
NG c AT = 16 
THTC=OSIN (3) 

IS 10 = u El N (-1) 

70=031 N (4) 

XC=DEIN { 5 ) 

Y C = 0 3 1 N { c ) 

2C=0EIN f 7 t 

7xc=v.j*occc ( isio -occ5 (psigi 

V Y C = V 0 * C C C o i Y r 7 «. } * 3 S 7 .i ( ? 3 1 j ; 

vzc=~* vc*diin cihto) 

NLHEHICAL IJIIZG3 ATIC In ( u T H-Cfi E ES SU N G E - SGT I' A) 

DC 75 *1=1,4 

T=nsiN { i ) 

;HI=D2 t J (2) 

P S I - 3 ~ I N (J) 

v=dsin (4) 

FHI= DEIN ( U) 

A X = A C 
P.1=?C 

i: ( te? in -gi- o .) h = cein n 5) 

IP { T A C C . 07 . J . } A X = l £ 1 N ( ib) 

DEC'JT (1) = 1 . 

QECUT k 2 ) = | A A * L CC 5 l P V. I ) - J* COOS (T HT) ) / 7 

Dzc T jr{ 3) = a:i*dsin (phi) /(7*dco5 ithu ) 

DEC'JT (4) - - G * C S I N 1 1 riT ) 

DECUr (5) = '.*DCC£ (ThT) *3C33 (P5I) 



j, is, 1 i 



ACC 



:equt 



ECJT (7) =-Y*CS:X (ThT) 



c£ i : m ) *331 n <psi; 



3 i C U i ( _ 
Dr CUT ( 9 



“Aia\^ s CiAc ( — T X = T ) 
:£TY0-EiXP { - CTY X T ) 



DECUT ( 1C) 


= at::c*d 


DZCUT 


(11) 


*CSIN (c 


EECUT 


< 12) 


-DSTN ( ~ 


DECUT 


13 


= 33 I N ( 1 


DECUT 


(14) 


= PH 


D EC IT 


( 15) 


= 0- 


D SCUT 


(16 


= J • 



I? (T5? IN .GI- C . ) DECOT ( 1 5 ) = (PC - E 3)/7S PIN 

I? (T ACC. GT. 3 . ) C ECUT ( To ) = (AC- ?'/.) / TACC 

DC 7 d l = 1 , N G E A T 

IF Ml. E C • ^ ) GC IC 60 

IF (111 . SQ .3} GC TC 65 

IF (21. EC- 4) GC IC 70 

DEI (I) = DEIN (I) 

CF2 =DrCUI (I) *CI 
CC5l = DE2 (I) *.5 
GC TO 75 

DSE1=3ZCUT (I) * 2. *C? 

CE2 (I) = D E 2 (I) * l£E1 
US2t=*J521*.25 
GC TO 75 

US£1 = DHCDT (I) ’£.*11 
D22(I) =352(1 + C£E1 
usei=;jSF i*.5 
GC TO 75 

C£F1=DECCT (I) ^CT 
USE 1= (CE2 (I) *U£E 1) /o . 

D F IN (I) = DE1 ( I ) -LSI 1 



IF (TS?IN,IE. G, 
IF (TACC. IF.O . ) 



DEIN <15) = PC 
Ci IN ( 16) = AC 



THT=D2IN (2) 

P£I=D3IN (3) 

v — czi :i i-o 

VX= V=QC0 E (THT ) *DCCS (£31) 
7Y=7-3CG£ (THT) *CSIN (TSI) 
7Z=-7*D5IX (THT) 
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3 (1) 


- -> 3 1 >; 1 5 i - 


( > c + :< o * 3 1 ) 


2 U 


= 33IN id) - 


| : C V VO *CT j 


2 


= D2i:J (7) - 


(2C-7ZO-ZT) 


c <U) 


= v * > 0 




E (5) 


= V/- VYQ 




B ( 6) 


=7Z- VZO 




5 (7) 


-0 . 




B { c] 


= 0 - 




3 (9) 


=■) . 




B 3 T o 


i?.N 





= sc 
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50 



100 

15C 



SUE30JTINS ?I IT 5 ? < X , 2 , 3 , 0 , 0) 

IXPLIO IT 3i.\L*6 [>-:\,Z-L) 

DISELS ION A ( 9 , 9 ) , A T { i , :? ) , X ( 9 ) ( c ( 3 ) , ? 
DIMENSION A? AT (5,^) f XX ( 9) ,??(;,*) ,H ( 
C I 2 £. .N 3 I C N ri ? H ’[ ( 6 , c ) ,5 (o , ■: ) , i ( b , -3 ) , j ( 
rir.SN3i.CN c a ( 9 ) , 1 9 , 9) , o z ( 9 , 9 ) , ~ 
ccrroN/CNE/ a, at, del, *, a 

E XI 3 A? OLA 2E 01 ATT 7ZC 109 

CALL 7 MUTES ( A , A , 9 # 9 , 1 ,2 , 9 ,A X , 9 ,1 1 ) 

DO 50 1=1,9 
XX (I) = AX U) "2 (I) 

CCNTIM UZ 

E X I ?. A ? 0 L A Z £ 0 C \ A 5 I A N C Z 

CALL 7 MULE? ( ? , A I , : , 9 ,9 , 9 , 9 , 2 A T , 9 , 1 2 ) 

CALL 7)1 CLEF (A , 9 AT , 9 , 9 ,9 , 9 ,9 , A 1 AT, 9 

DC 1 5 J 1 = 1,9 
DC 100 0=1,9 

?? (I,J) =Ar.\T (I ,0) C,J) 

CCNTINCE 
CCNT1N US 



p 


! 9 t 3 ) , . 

fc/6 ,-i-- 


(9, 91 ,. 


( 




( 


3,o) , 


' 6 ) , u ( r 


9 




< ( rO) 



in:) 
-■ ^ - { ■' 
O) , oz 
, 7 C is £ 



,13) 



rCa:i 6 AND HZ 







**2+X2**2+:<3**2) 
**2+ X 2**2) 

* X 5 * .< 3 ♦ X o 



250 1=1,6 
200 “ * “ 



, * D * C 

>.*C*C*C1*i:^*E1-(X1*Xl*SH-X2*:<;Z 

; . * d * d * d i * o i * e - \\ mmuxin. 

• 1 + X 2* X * * 5 2) 2 (D *0- X 3 ♦ X 3 ) 

-0* C *C 1 *l 1 +X 1 * X 1* £4) 
-D=D*C1*C1+X0*a2*Z4) 



* 22 ) 



{3 

(3 



20 0 
2 5 C 



K 


(I, 


01 


= 0. 




CCS 2 


i: 


:i u £ 




ccnii: 


i UE 




H 


n. 


i) 


= X 1*X3/ (Cl* 


C*C) 


H 


n, 


2 ! 


t = X 2 * X 3 / ( ci* 


C*C) 


H 


< 1 , 


3 


1 =-E 1/ (D*C> 


H 


( ^ , 


1) 


i =0 2/ ((3**5) 


* (C1=*3) ) 


B 


< ^ , 


2] 


1 =04/ ( (2**5) 


* (C1**j) ) 


E 


(2, 


o) 


= 05/ ( 0**5) 


*D 1 ) 


H 


( 2 , 


4] 


1 =X 1*X1 * X 3/ ( 


C* C *D* D 1 ) 


H 


(2 , 


5; 


i = X 2 *X2 *X 2/ ( 


l*C*C*0 1) 


H 


( 4 , 


6) 


i =D 1 *XJ / { l *0 


*2) 


a 


13, 


1) 


1 = -X2/(Cl*C1) 


a 


<3, 


2 ; 


i =:< V <d l »ri ) 




H 


(7, 


i] 


1 =ct, ( (E < * 3 ) 


* (C 1**4) ) 


H 


N, 


2 


) = C // ( (C**3) 


* (C 1**4) ) 


H 


(4, 


3] 


1 = (11*X2*>3* 


53) / ((0**3) 


H 


(4, 


4 ; 


1 = { - X 1 * X 2 ) / (E*C I'D 1) 


a 


<4, 




) = (X1*X2)/ (C 


* D i*D1) 


H 


<5, 




> =X 1/D 




H 


15, 




} =X2/0 





: 1**2) 



16 5 



r ^ / 
6 ) 
(o) 



no* j) ) 

HD^C) ) 



ono on o non n non n n non 



C 



30 0 
35 C 



40 0 
4 5 C 



5 C C 



55 C 



C 



60 0 
6 5 C 
C 

c 



h \o , 3) = X 3 / C 

H ( c , 1 ) “ ( D '* 0 * x 4 - 3 3 * X i ) / i j - 0* j ) 

H fb ,2) - ( 3 *D* < 5 - 5>: *X2 // ( 3- C* 0 
H (6,3) = ,C*D^:<6-i32-X3)/(i3*C=J'D> 

H 1 6 , — ) = X 1/3 
H (o,5) = X 2/ D 
H (c , o) = X J/D 

DC 350 1=1,0 
DC 300 o = 1,o 
HI (I Z J) = R |J,I) 

CC MINUS 
C C M 1 3 U E 

COMPUTE KALil A N G A X !« X AD 3 1 X 

C A 1 1 7 £ U i ? r (??,.’! i , 9 , r,o , 0 ,9 , PHI, 9,14) 

CALL 7 X ti L r F ( H , P H I , £ ,5,b ,6 , 9 , H E HT , o , I 5 ) 

DC 450 1=1,6 
CC 400 J=1,o 
X (1,0) = ri r H A (I , 0 ) <• 5 (I ,0) 

CONTINUE 
CC N II N UE 



INVEST Y SATED 

CALL L I N \ 1 F ( Y , c , o , YD 7 , 0 , NO R K , I S R ) 

CALL Y illS L E F (PHI, 7 1 A 7 , ,6 ,6,9 , c , G , 9 , 1 6 ) 
UPDATE STATE VECICL 



HH (1) = - D A T A 3 2 03 ,21) 
Hr. U) = ( (X 1-X 1 *X j*x-) * 
HH (3) = D A T A N 2 ( X2 , X 1) 

H H (4) - ( (-X1*X2’<4) f (X 
H H (5) = C 
HH (6) = D 2 / D 



(X 2 * x2* 5) * { L i*: i -d 

:*x2 s x 5 } )/ {D’lji + :i) 



DC SCO 1=1,6 
DC (I) =2 (I) -HH (I) 
CCNTTN UL 



CALL VXULE? (G , D 2 , S , 6 ,1,9, 6, OX ,9,17) 

DC 550 1=1,9 
X (I)*XX (I) OX (I) 

CCNIIN US 

UPDATE CC7ARI A NCE 

CALL VHUIFP (G ,K ,5 ,6 ,9 ,9 , 6 ,GH , 9 ,13) 

DC 650 1=1,9 
CC bOO J = 1 , 9 

CON II 3 UE 

CALL YHUIrr (F ,EF , 9 , 9 ,9,9 ,S, ?, 9 ,19) 

RETURN 

END 



O ) / (DD 
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c 



c 



c 



c 



c 



SUEPGCJTIN5 FHiiC (S’7 / C,DT r 5l,f2 / J3 / ^l 1 , „ 1 2 , w 1 3 , ± 3 - , 
IMPLICIT A E A > t ( A - H , u. - 2 ) 

V=£AT* + 2 
A = C*GT 
Z 1 = D E X ? (-A) 

E2=C2XF { - 2 . * A) 



V 1 1 = 7/ 

7 12=// { 2 4 •* 3 , 
7 1 3=7/ (C>‘* ') 
722 = 7/ v 2 **2) 
7 2 i = V / C 

V 3 3 = 7 




0 11 = 71 1* { 1.-2 
■;12 = 71 2» (52 4-1 
;i3=;u* M.-z 
V 2 2 = 72 2 * *£ 

223=723* { =2+ 1 


2 + 2 . * A *- (2. /3 .) *(A**3)-2,*A*A-*.*A*E1) 
. - 3 . ■’ H 1 *j: , * A * 2 1 - 2 . * A + k * A ) 

2 - i . * A * Z 1 ) 

1-i .-£2 '2. * A) 

. - 2 . * £ 1 } 



PZEuP/J 
Z N F 



.531 
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c 



IOC 
20 C 



SUE3CJTIN l ST AT (a , .i , 
IMPLICIT 3 2 A D * c (A-.i 
DIMENSION X (5 0) 

AN -S' 

SUJN= 0 . 

dc icc 

S U M = 3 o M + X ( £ ) 

CCNTIM 12 
A V G- S J M/ AN 

sc;i 2 = 0 - 

DC 203 1=1, j 
S U M 2 = 3 Uii2 + {X (I ) -AYS) 
CCMIii JE 

a X S = D S ^ D ‘I 4 S U M 2 / A N } 
PETUaN 
E N C 



/* 

//G0.3XSIN D D * 
/* 



.1 V J 
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